From 209170176d0d60839620c9c0c11afaad4dac163a Mon Sep 17 00:00:00 2001 From: roy Date: Tue, 9 Jul 2019 13:15:03 +0200 Subject: [PATCH 01/19] implemented battery --- .../revolve/gazebo/brains/Evaluator.cpp | 108 +++++++++++- cpprevolve/revolve/gazebo/motors/JointMotor.h | 61 +++---- .../revolve/gazebo/motors/PositionMotor.cpp | 6 +- .../revolve/gazebo/plugin/RobotController.cpp | 70 ++------ .../revolve/gazebo/plugin/RobotController.h | 25 +-- experiments/bo_learner/manager.py | 2 +- experiments/examples/manager.py | 6 +- pyrevolve/SDF/joint.py | 7 + pyrevolve/SDF/revolve_bot_sdf_builder.py | 4 + worlds/4_4m_walls.world | 158 ++++++++++++++++++ 10 files changed, 334 insertions(+), 113 deletions(-) create mode 100644 worlds/4_4m_walls.world diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp index e082cd1596..cc310ec715 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp @@ -47,7 +47,7 @@ Evaluator::Evaluator(const double _evaluationRate, this->current_position_.Reset(); this->previous_position_.Reset(); this->start_position_.Reset(); - this->locomotion_type = "directed"; // {directed, gait} + this->locomotion_type = "battery"; // {directed, gait, battery} this->path_length = 0.0; } @@ -163,6 +163,112 @@ double Evaluator::Fitness() (dist_projection / (alpha + ksi) - penalty); fitness_value = fitness_direction; } + else if (this->locomotion_type == "battery") + { + + // f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t - (len * size(13-34) * P) ) ] + double initial_battery = 1; // b -> initial battery level. 1 (100%) + double battery_time = 0.15; // b_t -> battery drainage, how much battery robot loses when idle (over time) + double distance_projection; // D_p -> distance projection, the path the robot takes + double shortest_distance; // D -> the shortest path to target + // len -> this->path_length + double robot_size = 17; // size -> the size of the robot TODO get the size from the robot manager + double power_used = 0.01; // P -> the amount of power used by each servo motor TODO calculate this with Torque * Angular Velocity + + // initializing the coordinates of the charging station (±1.6, ±1.6, 0.12) (x,y,z) + ignition::math::Pose3d target_coord; + target_coord.Pos().X() = 1.6; + target_coord.Pos().Y() = 1.6; + target_coord.Pos().Z() = 0.12; + // calculating the shortest/initial distance + shortest_distance = measure_distance(this->start_position_, target_coord); + std::cout << "b: " << initial_battery << "\n"; + std::cout << "b_t: " << battery_time << "\n"; + std::cout << "D: " << shortest_distance << "\n"; + std::cout << "size: " << robot_size << "\n"; + std::cout << "P: " << power_used << "\n"; + + + + // calculating the path length + this->step_poses.push_back(this->current_position_); + //step_poses: x y z roll pitch yaw + +// std::cout << "step_poses.size(): " << step_poses.size() << " "; + + for (int i=1; i < this->step_poses.size(); i++) + { + const auto &pose_i_1 = this->step_poses[i-1]; + const auto &pose_i = this->step_poses[i]; + this->path_length += Evaluator::measure_distance(pose_i_1, pose_i); + //save coordinations to coordinates.txt + std::ofstream coordinates; + coordinates.open(this->directory_name + "/coordinates.txt",std::ios::app); + + if(i == 1) + { + start_position_ = pose_i_1; + coordinates << std::fixed << start_position_.Pos().X() << " " << start_position_.Pos().Y() << std::endl; + } + coordinates << std::fixed << pose_i.Pos().X() << " " << pose_i.Pos().Y() << std::endl; + } + + std::cout << "len: " << this << "\n"; + + // calculating the distance projection + + ////********** directed locomotion fitness function **********//// + //directions(forward) of heads are the orientation(+x axis) - 1.570796 + double beta0 = this->start_position_.Rot().Yaw()- M_PI/2.0; + + if (beta0 < - M_PI) //always less than pi (beta0 + max(40degree) < pi) + { + beta0 = 2 * M_PI - std::abs(beta0); + } + + double beta1 = std::atan2( + this->current_position_.Pos().Y() - this->start_position_.Pos().Y(), + this->current_position_.Pos().X() - this->start_position_.Pos().X()); + + double alpha; + if (std::abs(beta1 - beta0) > M_PI) + { + alpha = 2 * M_PI - std::abs(beta1) - std::abs(beta0); + } else + { + alpha = std::abs(beta1 - beta0); + } + + double A = std::tan(beta0); + double B = this->start_position_.Pos().Y() + - A * this->start_position_.Pos().X(); + + double X_p = (A * (this->current_position_.Pos().Y() - B) + + this->current_position_.Pos().X()) / (A * A + 1); + double Y_p = A * X_p + B; + + //calculate the dist_projection + if (alpha > 0.5 * M_PI) + { + distance_projection = -std::sqrt( + std::pow((this->start_position_.Pos().X() - X_p), 2.0) + + std::pow((this->start_position_.Pos().Y() - Y_p), 2.0)); + } + else + { + distance_projection = std::sqrt( + std::pow((this->start_position_.Pos().X() - X_p), 2.0) + + std::pow((this->start_position_.Pos().Y() - Y_p), 2.0)); + } + // ************************ + std::cout << "D_p: " << distance_projection << "\n"; + + + + //f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t - (len * size(13-34) * P) ) ] + fitness_value = (distance_projection/shortest_distance) * initial_battery - + ((1 - (distance_projection/shortest_distance) ) * (battery_time + (this->path_length * robot_size * power_used))); + } return fitness_value; } diff --git a/cpprevolve/revolve/gazebo/motors/JointMotor.h b/cpprevolve/revolve/gazebo/motors/JointMotor.h index de780c4d8e..0e6739035f 100644 --- a/cpprevolve/revolve/gazebo/motors/JointMotor.h +++ b/cpprevolve/revolve/gazebo/motors/JointMotor.h @@ -25,35 +25,38 @@ #include -namespace revolve -{ - namespace gazebo - { - class JointMotor - : public Motor - { - /// \brief Constructor - /// \brief[in] _model Model identifier - /// \brief[in] _partId Module identifier - /// \brief[in] _motorId Motor identifier - /// \brief[in] _outputs Number of motor outputs - public: JointMotor( - ::gazebo::physics::ModelPtr _model, - const std::string &_partId, - const std::string &_motorId, - sdf::ElementPtr _motor, - const unsigned int _outputs); - - /// \brief Destructor - public: virtual ~JointMotor(); - - /// \brief The joint this motor is controlling - protected: ::gazebo::physics::JointPtr joint_; - - /// \brief Scoped name of the controlled joint - protected: std::string jointName_; - }; - } /* namespace gazebo */ +namespace revolve { +namespace gazebo { + +class JointMotor : public Motor { + /// \brief Constructor + /// \brief[in] _model Model identifier + /// \brief[in] _partId Module identifier + /// \brief[in] _motorId Motor identifier + /// \brief[in] _outputs Number of motor outputs +public: + JointMotor( + ::gazebo::physics::ModelPtr _model, + const std::string &_partId, + const std::string &_motorId, + sdf::ElementPtr _motor, + const unsigned int _outputs); + + /// \brief Destructor +public: + virtual ~JointMotor(); + + /// \brief The joint this motor is controlling +protected: + ::gazebo::physics::JointPtr joint_; + + /// \brief Scoped name of the controlled joint + std::string jointName_; + + /// \brief The id of the consumer + uint32_t consumerId_; +}; +} /* namespace gazebo */ } /* namespace revolve */ #endif /* REVOLVE_GAZEBO_MOTORS_JOINTMOTOR_H_ */ diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index 63a33724fe..f4ded7ebcc 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -127,8 +127,12 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) position += (position > 0 ? -2 * M_PI : 2 * M_PI); } + auto test = this->joint_->GetForceTorque(0); + auto error = position - this->positionTarget_; - auto cmd = this->pid_.Update(error, stepTime); + auto cmd = this->pid_.Update(error, stepTime); // angular velocity + std::cout << "~~~~~~ MOTORID: " << this->motorId_<< " ~~~~~\n"; + std::cout << "watts:\t" << cmd * test.body1Torque.Length() << "\tor\t" << cmd * test.body2Torque.Length() << "\n"; this->joint_->SetParam("vel", 0, cmd); } diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 495a691c6b..65fdd1ec19 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -62,13 +62,6 @@ void RobotController::Load( this->node_.reset(new gz::transport::Node()); this->node_->Init(); - // Subscribe to robot battery state updater - this->batterySetSub_ = this->node_->Subscribe( - "~/battery_level/request", - &RobotController::UpdateBattery, - this); - this->batterySetPub_ = this->node_->Advertise< gz::msgs::Response >( - "~/battery_level/response"); if (not _sdf->HasElement("rv:robot_config")) { @@ -105,34 +98,6 @@ void RobotController::Load( this->Startup(_parent, _sdf); } -///////////////////////////////////////////////// -void RobotController::UpdateBattery(ConstRequestPtr &_request) -{ - if (_request->data() not_eq this->model_->GetName() and - _request->data() not_eq this->model_->GetScopedName()) - { - return; - } - - gz::msgs::Response resp; - resp.set_id(_request->id()); - resp.set_request(_request->request()); - - if (_request->request() == "set_battery_level") - { - resp.set_response("success"); - this->SetBatteryLevel(_request->dbl_data()); - } - else - { - std::stringstream ss; - ss << this->BatteryLevel(); - resp.set_response(ss.str()); - } - - batterySetPub_->Publish(resp); -} - ///////////////////////////////////////////////// void RobotController::LoadActuators(const sdf::ElementPtr _sdf) { @@ -251,7 +216,8 @@ void RobotController::DoUpdate(const ::gazebo::common::UpdateInfo _info) { auto currentTime = _info.simTime.Double() - initTime_; - brain_->Update(motors_, sensors_, currentTime, actuationTime_); + this->brain_->Update(motors_, sensors_, currentTime, actuationTime_); + this->battery_->Update(); } ///////////////////////////////////////////////// @@ -259,26 +225,16 @@ void RobotController::LoadBattery(const sdf::ElementPtr _sdf) { if (_sdf->HasElement("rv:battery")) { - this->batteryElem_ = _sdf->GetElement("rv:battery"); - } -} - -///////////////////////////////////////////////// -double RobotController::BatteryLevel() -{ - if (not batteryElem_ or not batteryElem_->HasElement("rv:level")) - { - return 0.0; - } - - return batteryElem_->GetElement("rv:level")->Get< double >(); -} - -///////////////////////////////////////////////// -void RobotController::SetBatteryLevel(double _level) -{ - if (batteryElem_ and batteryElem_->HasElement("rv:level")) - { - batteryElem_->GetElement("rv:level")->Set(_level); + sdf::ElementPtr batteryElem = _sdf->GetElement("rv:battery"); + this->battery_.reset(new ::gazebo::common::Battery()); + this->battery_->UpdateParameters(batteryElem); + this->battery_->ResetVoltage(); + this->battery_->SetUpdateFunc([](const ::gazebo::common::BatteryPtr &battery) -> double { + std::cout << "battery: " << battery->Voltage() << "V" << std::endl; + for (const auto &consumer: battery->PowerLoads()) { + std::cout << "comsumer: " << consumer.first << " -> " << consumer.second << std::endl; + } + return battery->Voltage(); + }); } } diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.h b/cpprevolve/revolve/gazebo/plugin/RobotController.h index 6fe8e7bd69..2d7156ada9 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.h +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.h @@ -60,19 +60,6 @@ namespace revolve /// according to the update rate specified in the robot plugin. public: virtual void DoUpdate(const ::gazebo::common::UpdateInfo _info); - /// \brief Returns the battery level - /// \details Methods allows reading and writing the battery level in - /// the robot SDF. This is mostly useful for the `BatterySensor` to - /// obtain the battery state, and storing it in the SDF also means it - /// will be adequately backed up in an eventual snapshot. - public: double BatteryLevel(); - - /// \brief Sets the battery level if possible - public: void SetBatteryLevel(double _level); - - /// \brief Request listener for battery update - public: void UpdateBattery(ConstRequestPtr &_request); - /// \brief Detects and loads motors in the plugin spec protected: virtual void LoadActuators(const sdf::ElementPtr _sdf); @@ -101,12 +88,6 @@ namespace revolve /// \brief Networking node protected: ::gazebo::transport::NodePtr node_; - /// \brief Subscriber for battery update request - protected: ::gazebo::transport::SubscriberPtr batterySetSub_; - - /// \brief Responder for battery update request - protected: ::gazebo::transport::PublisherPtr batterySetPub_; - /// \brief Holds an instance of the motor factory protected: MotorFactoryPtr motorFactory_; @@ -122,9 +103,6 @@ namespace revolve /// \brief Time of initialisation protected: double initTime_; - /// \brief rv:battery element, if present - protected: sdf::ElementPtr batteryElem_; - /// \brief Time of the last actuation, in seconds and nanoseconds protected: ::gazebo::common::Time lastActuationTime_; @@ -140,6 +118,9 @@ namespace revolve /// \brief Pointer to the world protected: ::gazebo::physics::WorldPtr world_; + /// \brief Pointer to the battery + protected: ::gazebo::common::BatteryPtr battery_; + /// \brief Driver update event pointer private: ::gazebo::event::ConnectionPtr updateConnection_; }; diff --git a/experiments/bo_learner/manager.py b/experiments/bo_learner/manager.py index f37fe45584..2349d79137 100755 --- a/experiments/bo_learner/manager.py +++ b/experiments/bo_learner/manager.py @@ -26,7 +26,7 @@ async def run(): # Load a robot from yaml robot = revolve_bot.RevolveBot() if settings.robot_yaml is None: - robot.load_file("experiments/bo_learner/yaml/spider.yaml") + robot.load_file("experiments/bo_learner/yaml/spider9.yaml") else: robot.load_file(settings.robot_yaml) robot.update_substrate() diff --git a/experiments/examples/manager.py b/experiments/examples/manager.py index 3dacc321c2..c637ad117c 100755 --- a/experiments/examples/manager.py +++ b/experiments/examples/manager.py @@ -14,6 +14,7 @@ from pyrevolve import parser from pyrevolve.SDF.math import Vector3 from pyrevolve.tol.manage import World +from pyrevolve.evolution import fitness async def run(): @@ -27,6 +28,7 @@ async def run(): robot = revolve_bot.RevolveBot() robot.load_file("experiments/examples/yaml/spider.yaml") robot.update_substrate() + robot.save_file("./spider.sdf", conf_type='sdf') # Connect to the simulator and pause world = await World.create(settings) @@ -45,8 +47,8 @@ async def run(): # Start a run loop to do some stuff while True: # Print robot fitness every second - print("Robot fitness is {fitness}".format( - fitness=robot_manager.fitness())) + fitness_=fitness.random(robot_manager) + print(f"Robot fitness is {fitness_}") await asyncio.sleep(1.0) diff --git a/pyrevolve/SDF/joint.py b/pyrevolve/SDF/joint.py index 220ebec0f5..0e62df33d9 100644 --- a/pyrevolve/SDF/joint.py +++ b/pyrevolve/SDF/joint.py @@ -36,6 +36,8 @@ def __init__(self, self.axis = JointAxis(axis) self.append(self.axis) + self.append(JointODEPhysics()) + def is_motorized(self): return self._motorized @@ -67,6 +69,11 @@ def to_robot_config_sdf(self): return servomotor +class JointODEPhysics(xml.etree.ElementTree.Element): + def __init__(self): + super().__init__('physics') + # ode = xml.etree.ElementTree.SubElement(self, 'ode') + SDF.sub_element_text(self, 'provide_feedback', True) class JointAxis(xml.etree.ElementTree.Element): def __init__(self, axis: SDF.math.Vector3): diff --git a/pyrevolve/SDF/revolve_bot_sdf_builder.py b/pyrevolve/SDF/revolve_bot_sdf_builder.py index fb59389a68..191c1ee4e2 100644 --- a/pyrevolve/SDF/revolve_bot_sdf_builder.py +++ b/pyrevolve/SDF/revolve_bot_sdf_builder.py @@ -294,4 +294,8 @@ def _sdf_brain_plugin_conf( if robot_genome is not None: SDF.sub_element_text(config, 'rv:genome', str(robot_genome)) + initial_charge = 10 + battery = xml.etree.ElementTree.SubElement(config, 'rv:battery') + SDF.sub_element_text(battery, 'voltage', initial_charge) + return plugin diff --git a/worlds/4_4m_walls.world b/worlds/4_4m_walls.world new file mode 100644 index 0000000000..06a5ee6476 --- /dev/null +++ b/worlds/4_4m_walls.world @@ -0,0 +1,158 @@ + + + + + 0 + 0 + + + true + + + + + 0 0 1 + 4 4 + + + + + + 100 + 50 + + + + 10 + + + false + + + 0 0 1 + 4 4 + + + + + + + 0 + 0 + 1 + + + + 1 + + 0 2 0.125 0 0 0 + + + + 4.1 0.1 0.25 + + + + + + + 4.1 0.1 0.25 + + + + + + + 0 -2 0.125 0 0 0 + + + + 4.1 0.1 0.25 + + + + + + + 4.1 0.1 0.25 + + + + + + + 2 0 0.125 0 0 1.57079632679 + + + + 4.1 0.1 0.25 + + + + + + + 4.1 0.1 0.25 + + + + + + + -2 0 0.125 0 0 1.57079632679 + + + + 4.1 0.1 0.25 + + + + + + + 4.1 0.1 0.25 + + + + + + + + + 0.005 + + + 0.0 + + + + + + + + 0.1 + 10e-6 + + + 100 + 1e-8 + + + world + + + + + + + + + model://sun + + + + + + From 6b08550434eefaacc7841c7808a307d3c2bede86 Mon Sep 17 00:00:00 2001 From: roy Date: Mon, 15 Jul 2019 15:39:17 +0200 Subject: [PATCH 02/19] implemented battery --- cpprevolve/revolve/gazebo/CMakeLists.txt | 1 + cpprevolve/revolve/gazebo/battery/Battery.cpp | 26 +++ cpprevolve/revolve/gazebo/battery/Battery.h | 42 ++++ .../revolve/gazebo/battery/battery_chart.py | 66 +++++++ .../revolve/gazebo/brains/DifferentialCPG.cpp | 7 +- .../revolve/gazebo/brains/DifferentialCPG.h | 6 +- .../revolve/gazebo/brains/Evaluator.cpp | 25 ++- cpprevolve/revolve/gazebo/brains/Evaluator.h | 7 +- cpprevolve/revolve/gazebo/brains/RLPower.cpp | 2 +- cpprevolve/revolve/gazebo/motors/JointMotor.h | 3 - cpprevolve/revolve/gazebo/motors/Motor.h | 114 ++++++----- .../revolve/gazebo/motors/MotorFactory.cpp | 6 +- .../revolve/gazebo/motors/MotorFactory.h | 4 +- .../revolve/gazebo/motors/PositionMotor.cpp | 23 ++- .../revolve/gazebo/motors/PositionMotor.h | 2 +- .../revolve/gazebo/plugin/RobotController.cpp | 46 +++-- .../revolve/gazebo/plugin/RobotController.h | 5 +- experiments/bo_learner/manager.py | 17 ++ experiments/bo_learner/yaml/babyA.yaml | 3 + experiments/bo_learner/yaml/babyC.yaml | 3 + experiments/bo_learner/yaml/spider9.yaml | 3 + .../yaml/yaml_temp/spider9-0-0.yaml | 185 ++++++++++++++++++ .../bo_learner/yaml/yaml_temp/spider9-0.yaml | 185 ++++++++++++++++++ experiments/examples/manager.py | 15 +- experiments/roys_experiments/manager.py | 89 +++++++++ pyrevolve/SDF/revolve_bot_sdf_builder.py | 9 +- worlds/4_4m_walls.world | 10 + 27 files changed, 805 insertions(+), 99 deletions(-) create mode 100644 cpprevolve/revolve/gazebo/battery/Battery.cpp create mode 100644 cpprevolve/revolve/gazebo/battery/Battery.h create mode 100644 cpprevolve/revolve/gazebo/battery/battery_chart.py create mode 100644 experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml create mode 100644 experiments/bo_learner/yaml/yaml_temp/spider9-0.yaml create mode 100755 experiments/roys_experiments/manager.py diff --git a/cpprevolve/revolve/gazebo/CMakeLists.txt b/cpprevolve/revolve/gazebo/CMakeLists.txt index f318f55130..bd6f8dabcc 100644 --- a/cpprevolve/revolve/gazebo/CMakeLists.txt +++ b/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 diff --git a/cpprevolve/revolve/gazebo/battery/Battery.cpp b/cpprevolve/revolve/gazebo/battery/Battery.cpp new file mode 100644 index 0000000000..1dd916aa53 --- /dev/null +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -0,0 +1,26 @@ +// +// 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), watts_used(0) + {} + +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; + } + this->current_charge += sum * delta_time; // charge is measured in joules + std::ofstream b_info_file; + b_info_file.open("/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/battery/battery_info_0.txt", std::ios_base::app); + b_info_file << global_time << " " << sum << " " << current_charge << std::endl; +// std::cout << this->watts_used<< std::endl; +} diff --git a/cpprevolve/revolve/gazebo/battery/Battery.h b/cpprevolve/revolve/gazebo/battery/Battery.h new file mode 100644 index 0000000000..7acc082fba --- /dev/null +++ b/cpprevolve/revolve/gazebo/battery/Battery.h @@ -0,0 +1,42 @@ +// +// Created by Roy Basmacier on 2019-07-09. +// + +#ifndef REVOLVE_BATTERY_H +#define REVOLVE_BATTERY_H + + +#include +#include +#include +#include + +#include + +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 amount of watts used for all servos at a time + double watts_used; + + friend class Evaluator; +}; + +} +} + +#endif //REVOLVE_BATTERY_H diff --git a/cpprevolve/revolve/gazebo/battery/battery_chart.py b/cpprevolve/revolve/gazebo/battery/battery_chart.py new file mode 100644 index 0000000000..a12ee06b66 --- /dev/null +++ b/cpprevolve/revolve/gazebo/battery/battery_chart.py @@ -0,0 +1,66 @@ +import matplotlib +import matplotlib.pyplot as plt +import numpy as np + + + +from scipy.ndimage.filters import gaussian_filter1d + + +''' +MacOSX +TkAgg +TkCairo +''' + +def average(n, lst): + res = [] + for i in range(len(lst)//n): + res.append(sum(lst[i*n:(i+1)*n])/n) + return res + + +def draw_graph(x, y): + plt.title('Battery Plot') + # plt.plot(x, y) + plt.plot(x, gaussian_filter1d(y, sigma=100)) + plt.plot(x, gaussian_filter1d(y, sigma=300)) + plt.plot(x, gaussian_filter1d(y, sigma=500)) + plt.plot(x, gaussian_filter1d(y, sigma=1000)) + + plt.ylabel('energy used at instance (joules)') + plt.xlabel('time (s)') + plt.show() + +def draw_graphs(x, y1, y2): + plt.title('Battery Plot') + plt.subplot(2, 1, 1) + plt.plot(x, y1) + plt.ylabel('energy used at instance (joules)') + + plt.subplot(2, 1, 2) + plt.plot(x, y2) + plt.xlabel('time (s)') + plt.ylabel('total energy used (joules)') + plt.show() + +def main(): + # matplotlib.use('MacOSX') + global_time = [] + watts_used = [] + current_charge = [] + draw30 = True + with open('battery_info.txt', 'r') as file: + for line in file: + data = line.strip('\n').split(' ') + global_time.append(float(data[0])) + watts_used.append(float(data[1])) + current_charge.append(float(data[2])) + if float(data[0]) > 30 and draw30: + # draw_graphs(global_time, watts_used, current_charge) + draw30 = False + draw_graph(global_time, watts_used) + # draw_graphs(global_time, watts_used, current_charge) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 88d4a69bc1..84a2ad00e7 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/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"); @@ -353,7 +356,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; } diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.h b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.h index 2595f83621..0f10a2c67c 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.h +++ b/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_; diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp index cc310ec715..442732b39f 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp @@ -23,6 +23,8 @@ #include "Evaluator.h" +#include + using namespace revolve::gazebo; ///////////////////////////////////////////////// @@ -35,11 +37,14 @@ double Evaluator::measure_distance( } ///////////////////////////////////////////////// -Evaluator::Evaluator(const double _evaluationRate, - const double step_saving_rate) +Evaluator::Evaluator(std::shared_ptr<::revolve::gazebo::Battery> battery, + const double _evaluationRate, + const double step_saving_rate + ) : last_step_time(-1) , step_saving_rate(step_saving_rate) , step_poses(0) + , battery_(battery) { assert(_evaluationRate > 0 and "`_evaluationRate` should be greater than 0"); this->evaluation_rate_ = _evaluationRate; @@ -165,7 +170,6 @@ double Evaluator::Fitness() } else if (this->locomotion_type == "battery") { - // f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t - (len * size(13-34) * P) ) ] double initial_battery = 1; // b -> initial battery level. 1 (100%) double battery_time = 0.15; // b_t -> battery drainage, how much battery robot loses when idle (over time) @@ -173,7 +177,7 @@ double Evaluator::Fitness() double shortest_distance; // D -> the shortest path to target // len -> this->path_length double robot_size = 17; // size -> the size of the robot TODO get the size from the robot manager - double power_used = 0.01; // P -> the amount of power used by each servo motor TODO calculate this with Torque * Angular Velocity + double power_used = this->battery_->watts_used; // P -> the amount of power used by each servo motor // initializing the coordinates of the charging station (±1.6, ±1.6, 0.12) (x,y,z) ignition::math::Pose3d target_coord; @@ -182,10 +186,10 @@ double Evaluator::Fitness() target_coord.Pos().Z() = 0.12; // calculating the shortest/initial distance shortest_distance = measure_distance(this->start_position_, target_coord); - std::cout << "b: " << initial_battery << "\n"; - std::cout << "b_t: " << battery_time << "\n"; +// std::cout << "b: " << initial_battery << "\n"; +// std::cout << "b_t: " << battery_time << "\n"; std::cout << "D: " << shortest_distance << "\n"; - std::cout << "size: " << robot_size << "\n"; +// std::cout << "size: " << robot_size << "\n"; std::cout << "P: " << power_used << "\n"; @@ -213,7 +217,7 @@ double Evaluator::Fitness() coordinates << std::fixed << pose_i.Pos().X() << " " << pose_i.Pos().Y() << std::endl; } - std::cout << "len: " << this << "\n"; + std::cout << "len: " << this->path_length << "\n"; // calculating the distance projection @@ -261,13 +265,16 @@ double Evaluator::Fitness() std::pow((this->start_position_.Pos().Y() - Y_p), 2.0)); } // ************************ - std::cout << "D_p: " << distance_projection << "\n"; + std::cout << "D_p: " << distance_projection << "\t"; + std::cout << "D: " << shortest_distance << "ratio:" << distance_projection/shortest_distance << "\n"; //f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t - (len * size(13-34) * P) ) ] fitness_value = (distance_projection/shortest_distance) * initial_battery - ((1 - (distance_projection/shortest_distance) ) * (battery_time + (this->path_length * robot_size * power_used))); + std::cout << "fitness: " << fitness_value << "\n"; + this->battery_->watts_used = 0.0; } return fitness_value; } diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.h b/cpprevolve/revolve/gazebo/brains/Evaluator.h index 357634da57..252b028d59 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.h +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.h @@ -24,6 +24,8 @@ #include +#include + namespace revolve { namespace gazebo @@ -31,7 +33,8 @@ namespace revolve class Evaluator { /// \brief Constructor - public: Evaluator(const double _evaluationRate, + public: Evaluator(std::shared_ptr<::revolve::gazebo::Battery> battery, + const double _evaluationRate, const double step_saving_rate = 0.1); /// \brief Destructor @@ -66,6 +69,8 @@ namespace revolve /// \brief Current position of a robot protected: ignition::math::Pose3d current_position_; + /// \brief Shared battery pointer + std::shared_ptr<::revolve::gazebo::Battery> battery_; /// \brief protected: double evaluation_rate_; diff --git a/cpprevolve/revolve/gazebo/brains/RLPower.cpp b/cpprevolve/revolve/gazebo/brains/RLPower.cpp index cdcd58fb49..836451b83b 100644 --- a/cpprevolve/revolve/gazebo/brains/RLPower.cpp +++ b/cpprevolve/revolve/gazebo/brains/RLPower.cpp @@ -75,7 +75,7 @@ RLPower::RLPower( this->InitialisePolicy(numMotors); // Start the evaluator - this->evaluator_.reset(new Evaluator(this->evaluationRate_)); + //this->evaluator_.reset(new Evaluator(this->evaluationRate_/*TODO find the battery */)); } ///////////////////////////////////////////////// diff --git a/cpprevolve/revolve/gazebo/motors/JointMotor.h b/cpprevolve/revolve/gazebo/motors/JointMotor.h index 0e6739035f..5d2c6dc723 100644 --- a/cpprevolve/revolve/gazebo/motors/JointMotor.h +++ b/cpprevolve/revolve/gazebo/motors/JointMotor.h @@ -52,9 +52,6 @@ class JointMotor : public Motor { /// \brief Scoped name of the controlled joint std::string jointName_; - - /// \brief The id of the consumer - uint32_t consumerId_; }; } /* namespace gazebo */ } /* namespace revolve */ diff --git a/cpprevolve/revolve/gazebo/motors/Motor.h b/cpprevolve/revolve/gazebo/motors/Motor.h index 3441f61beb..5bac28c280 100644 --- a/cpprevolve/revolve/gazebo/motors/Motor.h +++ b/cpprevolve/revolve/gazebo/motors/Motor.h @@ -26,64 +26,74 @@ #include #include - +#include namespace revolve { namespace gazebo { class Motor { - /// \brief Constructor - /// \brief[in] _model Model identifier - /// \brief[in] _partId Module identifier - /// \brief[in] _motorId Motor identifier - /// \brief[in] _outputs Number of motor outputs - public: Motor( - ::gazebo::physics::ModelPtr _model, - const std::string &_partId, - const std::string &_motorId, - const unsigned int _outputs); - - /// \brief Destructor - public: virtual ~Motor(); - - /// \brief Updates the motor based on the attached output of the neural - /// network. - /// \param[in,out] output Raw motor update value(s), it is up to the - /// motor to decide how to interpret this. This is a pointer to an - /// array of values, out of which the motor should read the first `n` - /// values if it specifies `n` outputs. - /// \param[in] step Actuation time in seconds - public: virtual void Update( - const double *_output, - double _step) = 0; - - /// \brief Retrieve the ID - /// \return The part ID - public: std::string PartId(); - - /// \return The full ID of the motor (should be unique in the robot) - public: std::string MotorId(); - - /// \return Number of output neurons connected to this motor - public: unsigned int Outputs(); - - /// \brief Create PID element - /// \param pid Pointer to the rv:pid element - /// \return Gazebo PID - public: static ::gazebo::common::PID CreatePid(sdf::ElementPtr _sdfPID); - - /// \brief The model this motor is part of - protected: ::gazebo::physics::ModelPtr model_; - - /// \brief ID of the body part the motor belongs to - protected: std::string partId_; - - /// \brief Robot-wide unique motor ID - protected: std::string motorId_; - - /// \brief Number of output neurons that should be connected to the motor. - protected: unsigned int outputs_; + /// \brief Constructor + /// \brief[in] _model Model identifier + /// \brief[in] _partId Module identifier + /// \brief[in] _motorId Motor identifier + /// \brief[in] _outputs Number of motor outputs + public: Motor( + ::gazebo::physics::ModelPtr _model, + const std::string &_partId, + const std::string &_motorId, + const unsigned int _outputs); + + /// \brief Destructor + public: virtual ~Motor(); + + /// \brief Updates the motor based on the attached output of the neural + /// network. + /// \param[in,out] output Raw motor update value(s), it is up to the + /// motor to decide how to interpret this. This is a pointer to an + /// array of values, out of which the motor should read the first `n` + /// values if it specifies `n` outputs. + /// \param[in] step Actuation time in seconds + public: virtual void Update( + const double *_output, + double _step) = 0; + + /// \brief Retrieve the ID + /// \return The part ID + public: std::string PartId(); + + /// \return The full ID of the motor (should be unique in the robot) + public: std::string MotorId(); + + /// \return Number of output neurons connected to this motor + public: unsigned int Outputs(); + + /// \brief Create PID element + /// \param pid Pointer to the rv:pid element + /// \return Gazebo PID + public: static ::gazebo::common::PID CreatePid(sdf::ElementPtr _sdfPID); + + /// \brief The model this motor is part of + protected: + ::gazebo::physics::ModelPtr model_; + + /// \brief ID of the body part the motor belongs to + std::string partId_; + + /// \brief Robot-wide unique motor ID + std::string motorId_; + + /// \brief Number of output neurons that should be connected to the motor. + unsigned int outputs_; + + + /// \brief Pointer to the battery + std::shared_ptr<::revolve::gazebo::Battery> battery_; + + /// \brief The id of the consumer + uint32_t consumerId_; + + friend class MotorFactory; }; } /* namespace gazebo */ } /* namespace tol_robogen */ diff --git a/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp b/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp index 0aee478484..0aac71ab61 100644 --- a/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp +++ b/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp @@ -22,6 +22,7 @@ #include #include +#include namespace gz = gazebo; using namespace revolve::gazebo; @@ -56,7 +57,7 @@ MotorPtr MotorFactory::Motor( } ///////////////////////////////////////////////// -MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf) +MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf, std::shared_ptr<::revolve::gazebo::Battery> battery) { auto typeParam = _motorSdf->GetAttribute("type"); auto partIdParam = _motorSdf->GetAttribute("part_id"); @@ -82,5 +83,8 @@ MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf) throw std::runtime_error("Motor error"); } +// adding consumer id to motor ptr from battery so each servo is a consumer of the battery + motor->battery_ = battery; + motor->consumerId_ = battery->AddConsumer(); return motor; } diff --git a/cpprevolve/revolve/gazebo/motors/MotorFactory.h b/cpprevolve/revolve/gazebo/motors/MotorFactory.h index f9863d7151..abfa800c22 100644 --- a/cpprevolve/revolve/gazebo/motors/MotorFactory.h +++ b/cpprevolve/revolve/gazebo/motors/MotorFactory.h @@ -26,7 +26,7 @@ #include #include - +#include namespace revolve { namespace gazebo @@ -54,7 +54,7 @@ namespace revolve const std::string &_motorId); /// \brief Creates a motor for the given model for the given SDF element. - public: virtual MotorPtr Create(sdf::ElementPtr _motorSdf); + public: virtual MotorPtr Create(sdf::ElementPtr _motorSdf, std::shared_ptr<::revolve::gazebo::Battery>); /// \brief Internal reference to the robot model protected: ::gazebo::physics::ModelPtr model_; diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index f4ded7ebcc..1f315a6502 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -30,7 +30,7 @@ PositionMotor::PositionMotor( gz::physics::ModelPtr _model, const std::string &_partId, const std::string &_motorId, - const sdf::ElementPtr _motor) + const sdf::ElementPtr &_motor) : JointMotor(std::move(_model), _partId, _motorId, _motor, 1) , positionTarget_(0) , noise_(0) @@ -78,7 +78,8 @@ PositionMotor::~PositionMotor() = default; ///////////////////////////////////////////////// void PositionMotor::Update( const double *outputs, - double /*step*/) + double /*step*/ + ) { // Just one network output, which is the first auto output = outputs[0]; @@ -127,12 +128,22 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) position += (position > 0 ? -2 * M_PI : 2 * M_PI); } - auto test = this->joint_->GetForceTorque(0); auto error = position - this->positionTarget_; - auto cmd = this->pid_.Update(error, stepTime); // angular velocity + auto cmd = this->pid_.Update(error, stepTime); // angular velocity TODO this is targeted velocisty - std::cout << "~~~~~~ MOTORID: " << this->motorId_<< " ~~~~~\n"; - std::cout << "watts:\t" << cmd * test.body1Torque.Length() << "\tor\t" << cmd * test.body2Torque.Length() << "\n"; + if (this->battery_) + { + ::gazebo::physics::JointWrench jointWrench = this->joint_->GetForceTorque(0); +// std::cout << "dot (local): " << this->joint_->LocalAxis(0).Dot(jointWrench.body1Torque) * cmd << '\t'; // dot product of torque and local axis + +// std::cout << "dot (global): " << this->joint_->GlobalAxis(0).Dot(jointWrench.body1Torque) * cmd << std::endl; + // TODO find wich axis to use local or global + // TODO check the watt if it should be positive or negative + // TODO change this for now im using the absolute value of the watt so it always decreases from the joint movements + double watt = -abs(cmd * jointWrench.body1Torque.Length()); // TODO check which torque to use 1 or 2 + this->battery_->SetPowerLoad(this->consumerId_ , watt); + + } this->joint_->SetParam("vel", 0, cmd); } diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.h b/cpprevolve/revolve/gazebo/motors/PositionMotor.h index 7f07c831a5..3b891d4512 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.h +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.h @@ -44,7 +44,7 @@ namespace revolve ::gazebo::physics::ModelPtr _model, const std::string &_partId, const std::string &_motorId, - const sdf::ElementPtr _motor); + const sdf::ElementPtr &_motor); /// \brief Destructor public: virtual ~PositionMotor() override; diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 65fdd1ec19..3e1828778e 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include "RobotController.h" @@ -79,6 +80,9 @@ void RobotController::Load( this->actuationTime_ = 1.0 / updateRate; } + // Call the battery loader + this->LoadBattery(robotConfiguration); + // Load motors this->motorFactory_ = this->MotorFactory(_parent); this->LoadActuators(robotConfiguration); @@ -91,9 +95,6 @@ void RobotController::Load( // can potentially be reordered. this->LoadBrain(robotConfiguration); - // Call the battery loader - this->LoadBattery(robotConfiguration); - // Call startup function which decides on actuation this->Startup(_parent, _sdf); } @@ -114,7 +115,7 @@ void RobotController::LoadActuators(const sdf::ElementPtr _sdf) auto servomotor = actuators->GetElement("rv:servomotor"); while (servomotor) { - auto servomotorObj = this->motorFactory_->Create(servomotor); + auto servomotorObj = this->motorFactory_->Create(servomotor, this->battery_); motors_.push_back(servomotorObj); servomotor = servomotor->GetNextElement("rv:servomotor"); } @@ -180,7 +181,7 @@ void RobotController::LoadBrain(const sdf::ElementPtr _sdf) } else if ("bo" == learner and "cpg" == controller) { - brain_.reset(new DifferentialCPG(this->model_, _sdf, motors_, sensors_)); + brain_.reset(new DifferentialCPG(this->model_, _sdf, motors_, sensors_, this->battery_)); } else { @@ -214,10 +215,23 @@ void RobotController::CheckUpdate(const ::gazebo::common::UpdateInfo _info) /// Default update function simply tells the brain to perform an update void RobotController::DoUpdate(const ::gazebo::common::UpdateInfo _info) { + ///TODO fix this when you have the right amount of initial charge for robots +// if (battery_->current_charge < 0) +// { +// std::exit(0); +// } + + auto currentTime = _info.simTime.Double() - initTime_; + /// exits out of simulation after 30 mins of simulation time +// if (initTime_ > 30) +// { +// std::exit(0); +// } + this->brain_->Update(motors_, sensors_, currentTime, actuationTime_); - this->battery_->Update(); + this->battery_->Update(currentTime, actuationTime_); } ///////////////////////////////////////////////// @@ -226,15 +240,19 @@ void RobotController::LoadBattery(const sdf::ElementPtr _sdf) if (_sdf->HasElement("rv:battery")) { sdf::ElementPtr batteryElem = _sdf->GetElement("rv:battery"); - this->battery_.reset(new ::gazebo::common::Battery()); + double battery_initial_charge; + try { + battery_initial_charge = std::stod( + batteryElem->GetAttribute("initial_charge")->GetAsString() + ); + } catch(std::invalid_argument &e) { + std::clog << "Initial charge of the robot not set, using 0.0" << std::endl; + battery_initial_charge = 0.0; + } + this->battery_.reset(new ::revolve::gazebo::Battery(battery_initial_charge)); // set initial battery (joules) this->battery_->UpdateParameters(batteryElem); this->battery_->ResetVoltage(); - this->battery_->SetUpdateFunc([](const ::gazebo::common::BatteryPtr &battery) -> double { - std::cout << "battery: " << battery->Voltage() << "V" << std::endl; - for (const auto &consumer: battery->PowerLoads()) { - std::cout << "comsumer: " << consumer.first << " -> " << consumer.second << std::endl; - } - return battery->Voltage(); - }); +// this->battery_->SetUpdateFunc([](const ::gazebo::common::BatteryPtr &battery) -> double { +// }); } } diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.h b/cpprevolve/revolve/gazebo/plugin/RobotController.h index 2d7156ada9..fdc761b5f9 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.h +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.h @@ -29,6 +29,7 @@ #include #include +#include namespace revolve { @@ -118,8 +119,8 @@ namespace revolve /// \brief Pointer to the world protected: ::gazebo::physics::WorldPtr world_; - /// \brief Pointer to the battery - protected: ::gazebo::common::BatteryPtr battery_; + /// \brief Shared pointer to the battery + protected: std::shared_ptr<::revolve::gazebo::Battery> battery_; /// \brief Driver update event pointer private: ::gazebo::event::ConnectionPtr updateConnection_; diff --git a/experiments/bo_learner/manager.py b/experiments/bo_learner/manager.py index 2349d79137..8896761d34 100755 --- a/experiments/bo_learner/manager.py +++ b/experiments/bo_learner/manager.py @@ -15,6 +15,8 @@ from pyrevolve.SDF.math import Vector3 from pyrevolve.tol.manage import World +from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor + async def run(): """ @@ -31,6 +33,21 @@ async def run(): robot.load_file(settings.robot_yaml) robot.update_substrate() + + # Start Simulator + if settings.simulator_cmd != 'debug': + simulator_supervisor = DynamicSimSupervisor( + world_file=settings.world, + simulator_cmd=settings.simulator_cmd, + simulator_args=["--verbose"], + plugins_dir_path=os.path.join('.', 'build', 'lib'), + models_dir_path=os.path.join('.', 'models'), + simulator_name='gazebo' + ) + await simulator_supervisor.launch_simulator(port=settings.port_start) + + + # Connect to the simulator and pause world = await World.create(settings) await world.pause(True) diff --git a/experiments/bo_learner/yaml/babyA.yaml b/experiments/bo_learner/yaml/babyA.yaml index 6ee67bdc64..e1420ea4e7 100644 --- a/experiments/bo_learner/yaml/babyA.yaml +++ b/experiments/bo_learner/yaml/babyA.yaml @@ -178,6 +178,9 @@ brain: acqui_ucb_alpha: 0.44 acqui_ei_jitter: 0 acquisition_function: "UCB" + gaussian_step_size: 0.3 + covrate: 0.4 + mutrate: 0.6 meta: robot_size: 26 run_analytics: "true" diff --git a/experiments/bo_learner/yaml/babyC.yaml b/experiments/bo_learner/yaml/babyC.yaml index 07b15884af..8448aa9939 100644 --- a/experiments/bo_learner/yaml/babyC.yaml +++ b/experiments/bo_learner/yaml/babyC.yaml @@ -324,6 +324,9 @@ brain: acqui_ucb_alpha: 0.44 acqui_ei_jitter: 0 acquisition_function: "UCB" + gaussian_step_size: 0.3 + covrate: 0.4 + mutrate: 0.6 meta: robot_size: 26 run_analytics: "true" diff --git a/experiments/bo_learner/yaml/spider9.yaml b/experiments/bo_learner/yaml/spider9.yaml index 7e153dceef..4f152974a0 100644 --- a/experiments/bo_learner/yaml/spider9.yaml +++ b/experiments/bo_learner/yaml/spider9.yaml @@ -170,6 +170,9 @@ brain: acqui_ucb_alpha: 0.44 acqui_ei_jitter: 0 acquisition_function: "UCB" + gaussian_step_size: 0.3 + covrate: 0.4 + mutrate: 0.6 meta: robot_size: 18 run_analytics: "true" diff --git a/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml b/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml new file mode 100644 index 0000000000..be1dc8c13a --- /dev/null +++ b/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml @@ -0,0 +1,185 @@ +--- +id: example_spider +body: + id : Core + type : Core + params: + red: 0.04 + green: 0.26 + blue: 0.72 + children : + 0: + id : Leg00Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + orientation : 90 + children : + 1: + id : Leg00 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : -90 + children : + 1: + id : Leg01Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + children : + 1: + id : Leg01 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : 0 + 1: + id : Leg10Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + orientation : 90 + children : + 1: + id : Leg10 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : -90 + children : + 1: + id : Leg11Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + children : + 1: + id : Leg11 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : 0 + 2: + id : Leg20Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + orientation : 90 + children : + 1: + id : Leg20 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : -90 + children : + 1: + id : Leg21Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + children : + 1: + id : Leg21 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : 0 + 3: + id : Leg30Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + orientation : 90 + children : + 1: + id : Leg30 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : -90 + children : + 1: + id : Leg31Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + children : + 1: + id : Leg31 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : 0 +brain: + type: bo-cpg + controller: + use_frame_of_reference: 0 + load_brain: "" + reset_neuron_state_bool: "true" + reset_neuron_random: "false" + abs_output_bound: 1.0 + signal_factor_all: 4.0 + signal_factor_mid: 2.5 + signal_factor_left_right: 2.5 + range_lb: -1.0 + range_ub: 1.5 + init_neuron_state: 0.707 + learner: + n_init_samples: 50 + init_method: "LHS" + kernel_noise: 0.00000001 + kernel_optimize_noise: "false" + kernel_sigma_sq: 0.222 + kernel_l: 0.55 + kernel_squared_exp_ard_k: 4 + acqui_gpucb_delta: 0.5 + acqui_ucb_alpha: 0.44 + acqui_ei_jitter: 0 + acquisition_function: "UCB" + gaussian_step_size: 0.3 + covrate: 0.4 + mutrate: 0.6 + meta: + robot_size: 18 + run_analytics: "true" + n_learning_iterations: 1450 + n_cooldown_iterations: 1 + reset_robot_position: "false" + evaluation_rate: 60 + output_directory: "output/cpg_bo/main_1562855507/0/1562855521.86/" + verbose: 1 + startup_time: 0 diff --git a/experiments/bo_learner/yaml/yaml_temp/spider9-0.yaml b/experiments/bo_learner/yaml/yaml_temp/spider9-0.yaml new file mode 100644 index 0000000000..9344de9abd --- /dev/null +++ b/experiments/bo_learner/yaml/yaml_temp/spider9-0.yaml @@ -0,0 +1,185 @@ +--- +id: example_spider +body: + id : Core + type : Core + params: + red: 0.04 + green: 0.26 + blue: 0.72 + children : + 0: + id : Leg00Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + orientation : 90 + children : + 1: + id : Leg00 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : -90 + children : + 1: + id : Leg01Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + children : + 1: + id : Leg01 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : 0 + 1: + id : Leg10Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + orientation : 90 + children : + 1: + id : Leg10 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : -90 + children : + 1: + id : Leg11Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + children : + 1: + id : Leg11 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : 0 + 2: + id : Leg20Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + orientation : 90 + children : + 1: + id : Leg20 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : -90 + children : + 1: + id : Leg21Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + children : + 1: + id : Leg21 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : 0 + 3: + id : Leg30Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + orientation : 90 + children : + 1: + id : Leg30 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : -90 + children : + 1: + id : Leg31Joint + type : ActiveHinge + params: + red: 0.98 + green: 0.98 + blue: 0.98 + children : + 1: + id : Leg31 + type : FixedBrick + params: + red: 0.04 + green: 0.26 + blue: 0.72 + orientation : 0 +brain: + type: bo-cpg + controller: + use_frame_of_reference: 0 + load_brain: "" + reset_neuron_state_bool: "true" + reset_neuron_random: "false" + abs_output_bound: 1.0 + signal_factor_all: 4.0 + signal_factor_mid: 2.5 + signal_factor_left_right: 2.5 + range_lb: -1.0 + range_ub: 1.5 + init_neuron_state: 0.707 + learner: + n_init_samples: 50 + init_method: "LHS" + kernel_noise: 0.00000001 + kernel_optimize_noise: "false" + kernel_sigma_sq: 0.222 + kernel_l: 0.55 + kernel_squared_exp_ard_k: 4 + acqui_gpucb_delta: 0.5 + acqui_ucb_alpha: 0.44 + acqui_ei_jitter: 0 + acquisition_function: "UCB" + gaussian_step_size: 0.3 + covrate: 0.4 + mutrate: 0.6 + meta: + robot_size: 18 + run_analytics: "true" + n_learning_iterations: 1450 + n_cooldown_iterations: 1 + reset_robot_position: "false" + evaluation_rate: 60 + output_directory: "" + verbose: 1 + startup_time: 0 diff --git a/experiments/examples/manager.py b/experiments/examples/manager.py index c637ad117c..66bab687c5 100755 --- a/experiments/examples/manager.py +++ b/experiments/examples/manager.py @@ -15,7 +15,7 @@ from pyrevolve.SDF.math import Vector3 from pyrevolve.tol.manage import World from pyrevolve.evolution import fitness - +from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor async def run(): """ @@ -30,6 +30,19 @@ async def run(): robot.update_substrate() robot.save_file("./spider.sdf", conf_type='sdf') + + # Start Simulator + if settings.simulator_cmd != 'debug': + simulator_supervisor = DynamicSimSupervisor( + world_file=settings.world, + simulator_cmd=settings.simulator_cmd, + simulator_args=["--verbose"], + plugins_dir_path=os.path.join('.', 'build', 'lib'), + models_dir_path=os.path.join('.', 'models'), + simulator_name='gazebo' + ) + await simulator_supervisor.launch_simulator(port=settings.port_start) + # Connect to the simulator and pause world = await World.create(settings) await world.pause(True) diff --git a/experiments/roys_experiments/manager.py b/experiments/roys_experiments/manager.py new file mode 100755 index 0000000000..f3213f5017 --- /dev/null +++ b/experiments/roys_experiments/manager.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +import os +import sys +import asyncio + +# Add `..` folder in search path +current_dir = os.path.dirname(os.path.abspath(__file__)) +newpath = os.path.join(current_dir, '..', '..') +sys.path.append(newpath) + +from pygazebo.pygazebo import DisconnectError + +from pyrevolve import revolve_bot +from pyrevolve import parser +from pyrevolve.SDF.math import Vector3 +from pyrevolve.tol.manage import World +from pyrevolve.evolution import fitness +from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor + + +async def run(): + """ + The main coroutine, which is started below. + """ + # Parse command line / file input arguments + settings = parser.parse_args() + + # Load a robot from yaml + robot = revolve_bot.RevolveBot() + robot.load_file("experiments/bo_learner/yaml/spider9.yaml") + robot.update_substrate() + robot.save_file("./spider.sdf", conf_type='sdf') + + + # Start Simulator + if settings.simulator_cmd != 'debug': + simulator_supervisor = DynamicSimSupervisor( + world_file=settings.world, + simulator_cmd=settings.simulator_cmd, + simulator_args=["--verbose"], + plugins_dir_path=os.path.join('.', 'build', 'lib'), + models_dir_path=os.path.join('.', 'models'), + simulator_name='gazebo' + ) + await simulator_supervisor.launch_simulator(port=settings.port_start) + + # Connect to the simulator and pause + world = await World.create(settings) + await world.pause(True) + + await (await world.delete_model(robot.id)) + await asyncio.sleep(2.5) + + # Insert the robot in the simulator + insert_future = await world.insert_robot(robot, Vector3(0, 0, 0.25)) + robot_manager = await insert_future + + # Resume simulation + await world.pause(False) + + # Start a run loop to do some stuff + while True: + # Print robot fitness every second + # fitness_=fitness(robot_manager) + # print(f"Robot fitness is {fitness_}") + await asyncio.sleep(1.0) + + +def main(): + def handler(loop, context): + exc = context['exception'] + if isinstance(exc, DisconnectError) \ + or isinstance(exc, ConnectionResetError): + print("Got disconnect / connection reset - shutting down.") + sys.exit(0) + raise context['exception'] + + try: + loop = asyncio.get_event_loop() + loop.set_exception_handler(handler) + loop.run_until_complete(run()) + except KeyboardInterrupt: + print("Got CtrlC, shutting down.") + + +if __name__ == '__main__': + print("STARTING") + main() + print("FINISHED") diff --git a/pyrevolve/SDF/revolve_bot_sdf_builder.py b/pyrevolve/SDF/revolve_bot_sdf_builder.py index 191c1ee4e2..b5cb2034a5 100644 --- a/pyrevolve/SDF/revolve_bot_sdf_builder.py +++ b/pyrevolve/SDF/revolve_bot_sdf_builder.py @@ -294,8 +294,11 @@ def _sdf_brain_plugin_conf( if robot_genome is not None: SDF.sub_element_text(config, 'rv:genome', str(robot_genome)) - initial_charge = 10 - battery = xml.etree.ElementTree.SubElement(config, 'rv:battery') - SDF.sub_element_text(battery, 'voltage', initial_charge) + #TODO get initial charge from revolve_bot + initial_charge = 0 + battery = xml.etree.ElementTree.SubElement(config, 'rv:battery', { + 'initial_charge': str(initial_charge), + }) + SDF.sub_element_text(battery, 'voltage', 0.0) return plugin diff --git a/worlds/4_4m_walls.world b/worlds/4_4m_walls.world index 06a5ee6476..8bc0b76181 100644 --- a/worlds/4_4m_walls.world +++ b/worlds/4_4m_walls.world @@ -56,6 +56,11 @@ + + 1.0 0.26 0.72 1.0 + 1.0 0.26 0.72 1.0 + 0.1 0.1 0.1 1.0 + 4.1 0.1 0.25 @@ -92,6 +97,11 @@ + + 0.04 1.0 0.72 1.0 + 0.04 1.0 0.72 1.0 + 0.1 0.1 0.1 1.0 + 4.1 0.1 0.25 From 03ffcadb8cb2de96192cd970ba0ee46bb3044780 Mon Sep 17 00:00:00 2001 From: roy Date: Tue, 16 Jul 2019 14:32:19 +0200 Subject: [PATCH 03/19] implemented battery --- cpprevolve/revolve/gazebo/battery/Battery.cpp | 2 +- .../revolve/gazebo/brains/Evaluator.cpp | 11 +- experiments/bo_learner/GridSearch.py | 132 +++++++++--------- experiments/bo_learner/manager.py | 2 +- experiments/bo_learner/yaml/spider9.yaml | 2 +- .../yaml/yaml_temp/spider9-0-0.yaml | 2 +- pyrevolve/util/supervisor/supervisor_multi.py | 14 +- 7 files changed, 88 insertions(+), 77 deletions(-) diff --git a/cpprevolve/revolve/gazebo/battery/Battery.cpp b/cpprevolve/revolve/gazebo/battery/Battery.cpp index 1dd916aa53..4712307d29 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.cpp +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -20,7 +20,7 @@ void Battery::Update(double global_time, double delta_time) } this->current_charge += sum * delta_time; // charge is measured in joules std::ofstream b_info_file; - b_info_file.open("/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/battery/battery_info_0.txt", std::ios_base::app); + b_info_file.open("/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/battery/battery_info_spider9_0.txt", std::ios_base::app); b_info_file << global_time << " " << sum << " " << current_charge << std::endl; // std::cout << this->watts_used<< std::endl; } diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp index 442732b39f..0e87d4d79e 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp @@ -52,7 +52,7 @@ Evaluator::Evaluator(std::shared_ptr<::revolve::gazebo::Battery> battery, this->current_position_.Reset(); this->previous_position_.Reset(); this->start_position_.Reset(); - this->locomotion_type = "battery"; // {directed, gait, battery} + this->locomotion_type = "directed"; // {directed, gait, battery} this->path_length = 0.0; } @@ -83,7 +83,6 @@ double Evaluator::Fitness() } else if (this->locomotion_type == "directed") { - this->step_poses.push_back(this->current_position_); //step_poses: x y z roll pitch yaw for (int i=1; i < this->step_poses.size(); i++) @@ -188,7 +187,7 @@ double Evaluator::Fitness() shortest_distance = measure_distance(this->start_position_, target_coord); // std::cout << "b: " << initial_battery << "\n"; // std::cout << "b_t: " << battery_time << "\n"; - std::cout << "D: " << shortest_distance << "\n"; +// std::cout << "D: " << shortest_distance << "\n"; // std::cout << "size: " << robot_size << "\n"; std::cout << "P: " << power_used << "\n"; @@ -217,8 +216,8 @@ double Evaluator::Fitness() coordinates << std::fixed << pose_i.Pos().X() << " " << pose_i.Pos().Y() << std::endl; } - std::cout << "len: " << this->path_length << "\n"; - +// std::cout << "len: " << this->path_length << "\n"; +// // calculating the distance projection ////********** directed locomotion fitness function **********//// @@ -266,7 +265,7 @@ double Evaluator::Fitness() } // ************************ std::cout << "D_p: " << distance_projection << "\t"; - std::cout << "D: " << shortest_distance << "ratio:" << distance_projection/shortest_distance << "\n"; + std::cout << "D: " << shortest_distance << " ratio:" << distance_projection/shortest_distance << "\n"; diff --git a/experiments/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index 71f8c0f2c9..7a4717c157 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -1,10 +1,3 @@ -""" - experiments = [ - {'range_ub': 1.0, 'signal_factor_all': 1.0}, - {'range_ub': 1.0, 'signal_factor_all': 3.0}, - {'range_ub': 4.5, 'signal_factor_all': 3.0} - ] -""" from sys import platform import matplotlib if platform == "darwin": @@ -19,36 +12,63 @@ # Parameters -min_lines = 1480 -run_gazebo = False -n_runs = 15 # Naar 20 -n_jobs = 30 +targeted = True +n_objects = 10 +min_lines = 0 # This is a bit different with targeted flow +run_gazebo = True +n_runs = 1 +run_factor = 1 +n_jobs = 1 my_yaml_path = "experiments/bo_learner/yaml/" -yaml_model = "babyC.yaml" +yaml_model = "spider9.yaml" manager = "experiments/bo_learner/manager.py" python_interpreter = ".venv/bin/python3" +start_port = 12000 search_space = { - 'n_learning_iterations': [1500], - 'n_init_samples': [20], - 'evaluation_rate': [60], - 'verbose': [0], - 'kernel_sigma_sq': [1], - 'kernel_l': [0.02, 0.05, 0.1, 0.2], - 'acqui_ucb_alpha': [0.1, 0.3, 0.5, 1.0], - 'range_ub': [1.5], - 'signal_factor_all': [4.0], + 'verbose': [1], + # 'for_slower_amplitude_factor': [3.0], + # 'load_brain': ["'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378190.71/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389414.36/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378245.76/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378287.77/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378160.67/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389312.53/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378225.74/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389379.72/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389361.89/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389338.31/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389234.1/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378190.49/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378165.66/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389121.16/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389123.4/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378444.49/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378241.33/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389526.78/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389088.1/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389193.31/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389159.72/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378109.28/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389284.85/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378216.32/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389419.81/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378267.39/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378104.54/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378235.74/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389173.43/best_brain1501.txt'", + # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389430.53/best_brain1501.txt'", + # ] } print(search_space) # You don't have to change this my_sub_directory = "yaml_temp/" output_path = "output/cpg_bo/main_" + str(round(time.time())) + "/" -start_port = 11000 finished = False # Make in revolve/build to allow runs from terminal -os.system('cmake /home/gongjinlan/projects/revolve/ -DCMAKE_BUILD_TYPE="Release"') -os.system("make -j60") +os.system('mkdir build9 && cd build9 && cmake /Users/roy/projects/revolve -DCMAKE_BUILD_TYPE="Release"') +os.system("make -j4") def change_parameters(original_file, parameters): # Iterate over dictionary elements @@ -93,7 +113,7 @@ def create_yamls(yaml_path, model, sub_directory, experiments): def run(i, sub_directory, model, params): # Sleepy time when starting up to save gazebo from misery if i < n_jobs: - time.sleep(5*i) + time.sleep(3.5*i) else: print("Todo: Make sure you are leaving 2 seconds in between firing " "gazebos") @@ -129,7 +149,7 @@ def run(i, sub_directory, model, params): if not run_gazebo: py_command = python_interpreter + \ " ./revolve.py" + \ - " --manager " + manager + \ + - " --manager " + manager + \ " --world-address " + world_address + \ " --robot-yaml " + yaml_model else: @@ -138,7 +158,7 @@ def run(i, sub_directory, model, params): " --manager " + manager + \ " --world-address " + world_address + \ " --robot-yaml " + yaml_model + \ - " --simulator-cmd gazebo" \ + " --simulator-cmd gazebo" return_code = os.system(py_command) if return_code == 32512: @@ -149,36 +169,6 @@ def run(i, sub_directory, model, params): # Get permutations keys, values = zip(*search_space.items()) experiments = [dict(zip(keys, v)) for v in itertools.product(*values)] - - # PASTE THE EXPERIMENTS HERE, IN THE FORMAT SHOWN BELOW - experiments = [ - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, # BASE RUN - - # BASE RUN - {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - {'init_method': "LHS", 'kernel_l': 0.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - {'init_method': "LHS", 'kernel_l': 1.0, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - {'init_method': "LHS", 'kernel_l': 1.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.1, 'acqui_ucb_alpha': 0.5}, - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.2, 'acqui_ucb_alpha': 0.5}, - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.5, 'acqui_ucb_alpha': 0.5}, - # BASE RUN - - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.1}, - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.2}, - # BASE RUN - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.0}, - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.5}, - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 2.0}, - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0}, - {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 4.0}, - - {'init_method': "RS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - # BASE RUN - ] - # 'kernel_l': [0.02, 0.05, 0.1, 0.2], - # 'acqui_ucb_alpha': [0.1, 0.3, 0.5, 1.0], unique_experiments = experiments n_unique_experiments = len(experiments) @@ -186,6 +176,9 @@ def run(i, sub_directory, model, params): for ix, my_dict in enumerate(experiments): my_dict["id"] = ix + # Create a list with parameters to iterate over + experiments *= n_runs + # Save to yaml files create_yamls(yaml_path=my_yaml_path, model=yaml_model, @@ -209,9 +202,9 @@ def run(i, sub_directory, model, params): # Run experiments in parallel try: parallel(delayed(run)(i, - my_sub_directory, - yaml_model, - experiment) for i, experiment in enumerate(experiments)) + my_sub_directory, + yaml_model, + experiment) for i, experiment in enumerate(experiments)) except: print("Some runs are killed by timeout") @@ -224,12 +217,19 @@ def run(i, sub_directory, model, params): runs_succesful[str(e.split("/")[-2])] = 0 for my_run in runs: - if os.path.isfile(my_run + "fitnesses.txt"): - n_lines = len([(line.rstrip('\n')) for line in open(my_run + "fitnesses.txt")]) + if not targeted: + if os.path.isfile(my_run + "fitnesses.txt"): + n_lines = len([(line.rstrip('\n')) for line in open(my_run + "fitnesses.txt")]) + + # In case we had a succesful run + if n_lines > min_lines: + runs_succesful[str(e.split("/")[-2])] += 1 + else: + if os.path.isfile(my_run + "speed_to_object.txt"): + n_lines = len([(line.rstrip('\n')) for line in open(my_run + "speed_to_object.txt")]) + if n_lines >= n_objects: + runs_succesful[str(e.split("/")[-2])] += 1 - # In case we had a succesful run - if n_lines > min_lines: - runs_succesful[str(e.split("/")[-2])] += 1 to_run = {} for key, val in runs_succesful.items(): @@ -333,4 +333,4 @@ def run(i, sub_directory, model, params): params_string =[] for key, value in iter(search_space.items()): params_string += [str(key) + ": " + str(value)] - write_file(output_path + "search_space.txt", params_string) + write_file(output_path + "search_space.txt", params_string) \ No newline at end of file diff --git a/experiments/bo_learner/manager.py b/experiments/bo_learner/manager.py index 8896761d34..3ecdbd5ac8 100755 --- a/experiments/bo_learner/manager.py +++ b/experiments/bo_learner/manager.py @@ -49,7 +49,7 @@ async def run(): # Connect to the simulator and pause - world = await World.create(settings) + world = await World.create(settings, world_address=('localhost', settings.port_start)) await world.pause(True) await (await world.delete_model(robot.id)) diff --git a/experiments/bo_learner/yaml/spider9.yaml b/experiments/bo_learner/yaml/spider9.yaml index 4f152974a0..1df00f7444 100644 --- a/experiments/bo_learner/yaml/spider9.yaml +++ b/experiments/bo_learner/yaml/spider9.yaml @@ -181,5 +181,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: true startup_time: 0 diff --git a/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml b/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml index be1dc8c13a..4f3e523f9a 100644 --- a/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml +++ b/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml @@ -180,6 +180,6 @@ brain: n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 - output_directory: "output/cpg_bo/main_1562855507/0/1562855521.86/" + output_directory: "output/cpg_bo/main_1563198613/0/1563198803.71/" verbose: 1 startup_time: 0 diff --git a/pyrevolve/util/supervisor/supervisor_multi.py b/pyrevolve/util/supervisor/supervisor_multi.py index 88c7d186f1..51f7cda047 100644 --- a/pyrevolve/util/supervisor/supervisor_multi.py +++ b/pyrevolve/util/supervisor/supervisor_multi.py @@ -7,6 +7,7 @@ import psutil import sys import asyncio +import platform from datetime import datetime @@ -273,7 +274,18 @@ async def start_process(_ready_str, _output_tag, _address, _port): env = {} for key, value in os.environ.items(): env[key] = value - env['GAZEBO_MASTER_URI'] = 'http://{}:{}'.format(_address, _port) + env['GAZEBO_MASTER_URI'] = f'http://{_address}:{_port}' + + process = subprocess.run(['which', self.simulator_cmd[0]], stdout=subprocess.PIPE) + process.check_returncode() + gazebo_libraries_path = process.stdout.decode() + gazebo_libraries_path = os.path.dirname(gazebo_libraries_path) + gazebo_libraries_path = os.path.join(gazebo_libraries_path, '..', 'lib') + + if platform.system() == 'Darwin': + env['DYLD_LIBRARY_PATH'] = gazebo_libraries_path + else: # linux + env['LD_LIBRARY_PATH'] = gazebo_libraries_path self.procs[_output_tag] = await self._launch_with_ready_str( cmd=gz_args, ready_str=_ready_str, From 607c3d41b0b5cddb566c431b227c81665246d256 Mon Sep 17 00:00:00 2001 From: roy Date: Thu, 18 Jul 2019 15:21:55 +0200 Subject: [PATCH 04/19] adjusted for testing the battery levels --- cpprevolve/Makefile | 184 +++ cpprevolve/Revolve.cbp | 997 +++++++++++++ cpprevolve/cmake_install.cmake | 41 + cpprevolve/revolve/gazebo/Makefile | 1238 +++++++++++++++++ cpprevolve/revolve/gazebo/battery/Battery.cpp | 8 +- cpprevolve/revolve/gazebo/battery/Battery.h | 3 + .../revolve/gazebo/battery/battery_chart.py | 4 +- .../gazebo/battery/data/babyA/delete_this.txt | 0 .../gazebo/battery/data/babyB/delete_this.txt | 0 .../gazebo/battery/data/babyC/delete_this.txt | 0 .../battery/data/gecko12/delete_this.txt | 0 .../battery/data/gecko17/delete_this.txt | 0 .../battery/data/gecko7/delete_this.txt | 0 .../battery/data/spider13/delete_this.txt | 0 .../battery/data/spider17/delete_this.txt | 0 .../battery/data/spider9/delete_this.txt | 0 .../revolve/gazebo/brains/DifferentialCPG.cpp | 3 +- .../revolve/gazebo/brains/Evaluator.cpp | 2 +- cpprevolve/revolve/gazebo/cmake_install.cmake | 54 + experiments/bo_learner/GridSearch.py | 126 +- experiments/bo_learner/yaml/babyA.yaml | 6 +- experiments/bo_learner/yaml/spider9.yaml | 4 +- .../yaml/yaml_temp/spider9-0-0.yaml | 2 +- 23 files changed, 2597 insertions(+), 75 deletions(-) create mode 100644 cpprevolve/Makefile create mode 100644 cpprevolve/Revolve.cbp create mode 100644 cpprevolve/cmake_install.cmake create mode 100644 cpprevolve/revolve/gazebo/Makefile create mode 100644 cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/cmake_install.cmake diff --git a/cpprevolve/Makefile b/cpprevolve/Makefile new file mode 100644 index 0000000000..477e05ee68 --- /dev/null +++ b/cpprevolve/Makefile @@ -0,0 +1,184 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 3.14 + +# Default target executed when no arguments are given to make. +default_target: all + +.PHONY : default_target + +# Allow only one "make -f Makefile2" at a time, but pass parallelism. +.NOTPARALLEL: + + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + + +# A target that is always out of date. +cmake_force: + +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake + +# The command to remove a file. +RM = /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /Users/roy/projects/revolve + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /Users/roy/projects/revolve + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target install/local +install/local: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local + +# Special rule for the target install/local +install/local/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local/fast + +# Special rule for the target install/strip +install/strip: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip + +# Special rule for the target install/strip +install/strip/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip/fast + +# Special rule for the target install +install: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -P cmake_install.cmake +.PHONY : install + +# Special rule for the target install +install/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -P cmake_install.cmake +.PHONY : install/fast + +# Special rule for the target list_install_components +list_install_components: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" +.PHONY : list_install_components + +# Special rule for the target list_install_components +list_install_components/fast: list_install_components + +.PHONY : list_install_components/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache + +.PHONY : rebuild_cache/fast + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache + +.PHONY : edit_cache/fast + +# The main all target +all: cmake_check_build_system + cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -E cmake_progress_start /Users/roy/projects/revolve/CMakeFiles /Users/roy/projects/revolve/cpprevolve/CMakeFiles/progress.marks + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/all + $(CMAKE_COMMAND) -E cmake_progress_start /Users/roy/projects/revolve/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/clean +.PHONY : clean + +# The main clean target +clean/fast: clean + +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... install/local" + @echo "... install/strip" + @echo "... install" + @echo "... list_install_components" + @echo "... rebuild_cache" + @echo "... edit_cache" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/cpprevolve/Revolve.cbp b/cpprevolve/Revolve.cbp new file mode 100644 index 0000000000..950039aa92 --- /dev/null +++ b/cpprevolve/Revolve.cbp @@ -0,0 +1,997 @@ + + + + + + diff --git a/cpprevolve/cmake_install.cmake b/cpprevolve/cmake_install.cmake new file mode 100644 index 0000000000..e0488848e9 --- /dev/null +++ b/cpprevolve/cmake_install.cmake @@ -0,0 +1,41 @@ +# Install script for directory: /Users/roy/projects/revolve/cpprevolve + +# Set the install prefix +if(NOT DEFINED CMAKE_INSTALL_PREFIX) + set(CMAKE_INSTALL_PREFIX "/usr/local") +endif() +string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + if(BUILD_TYPE) + string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + else() + set(CMAKE_INSTALL_CONFIG_NAME "Debug") + endif() + message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +endif() + +# Set the component getting installed. +if(NOT CMAKE_INSTALL_COMPONENT) + if(COMPONENT) + message(STATUS "Install component: \"${COMPONENT}\"") + set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + else() + set(CMAKE_INSTALL_COMPONENT) + endif() +endif() + +# Is this installation the result of a crosscompile? +if(NOT DEFINED CMAKE_CROSSCOMPILING) + set(CMAKE_CROSSCOMPILING "FALSE") +endif() + +if(NOT CMAKE_INSTALL_LOCAL_ONLY) + # Include the install script for each subdirectory. + include("/Users/roy/projects/revolve/cpprevolve/revolve/brains/cmake_install.cmake") + include("/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/cmake_install.cmake") + +endif() + diff --git a/cpprevolve/revolve/gazebo/Makefile b/cpprevolve/revolve/gazebo/Makefile new file mode 100644 index 0000000000..118fc6781b --- /dev/null +++ b/cpprevolve/revolve/gazebo/Makefile @@ -0,0 +1,1238 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 3.14 + +# Default target executed when no arguments are given to make. +default_target: all + +.PHONY : default_target + +# Allow only one "make -f Makefile2" at a time, but pass parallelism. +.NOTPARALLEL: + + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + + +# A target that is always out of date. +cmake_force: + +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake + +# The command to remove a file. +RM = /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /Users/roy/projects/revolve + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /Users/roy/projects/revolve + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target list_install_components +list_install_components: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" +.PHONY : list_install_components + +# Special rule for the target list_install_components +list_install_components/fast: list_install_components + +.PHONY : list_install_components/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache + +.PHONY : rebuild_cache/fast + +# Special rule for the target install/strip +install/strip: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip + +# Special rule for the target install/strip +install/strip/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip/fast + +# Special rule for the target install +install: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -P cmake_install.cmake +.PHONY : install + +# Special rule for the target install +install/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -P cmake_install.cmake +.PHONY : install/fast + +# Special rule for the target install/local +install/local: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local + +# Special rule for the target install/local +install/local/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local/fast + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." + /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache + +.PHONY : edit_cache/fast + +# The main all target +all: cmake_check_build_system + cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -E cmake_progress_start /Users/roy/projects/revolve/CMakeFiles /Users/roy/projects/revolve/cpprevolve/revolve/gazebo/CMakeFiles/progress.marks + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/all + $(CMAKE_COMMAND) -E cmake_progress_start /Users/roy/projects/revolve/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/clean +.PHONY : clean + +# The main clean target +clean/fast: clean + +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +# Convenience name for target. +cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/rule: + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/rule +.PHONY : cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/rule + +# Convenience name for target. +RobotControlPlugin: cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/rule + +.PHONY : RobotControlPlugin + +# fast build rule for target. +RobotControlPlugin/fast: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build +.PHONY : RobotControlPlugin/fast + +# Convenience name for target. +cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/rule: + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/rule +.PHONY : cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/rule + +# Convenience name for target. +revolve-proto: cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/rule + +.PHONY : revolve-proto + +# fast build rule for target. +revolve-proto/fast: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build +.PHONY : revolve-proto/fast + +# Convenience name for target. +cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/rule: + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/rule +.PHONY : cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/rule + +# Convenience name for target. +WorldControlPlugin: cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/rule + +.PHONY : WorldControlPlugin + +# fast build rule for target. +WorldControlPlugin/fast: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build +.PHONY : WorldControlPlugin/fast + +# Convenience name for target. +cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/rule: + cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/rule +.PHONY : cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/rule + +# Convenience name for target. +revolve-gazebo: cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/rule + +.PHONY : revolve-gazebo + +# fast build rule for target. +revolve-gazebo/fast: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build +.PHONY : revolve-gazebo/fast + +battery/Battery.o: battery/Battery.cpp.o + +.PHONY : battery/Battery.o + +# target to build an object file +battery/Battery.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/battery/Battery.cpp.o +.PHONY : battery/Battery.cpp.o + +battery/Battery.i: battery/Battery.cpp.i + +.PHONY : battery/Battery.i + +# target to preprocess a source file +battery/Battery.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/battery/Battery.cpp.i +.PHONY : battery/Battery.cpp.i + +battery/Battery.s: battery/Battery.cpp.s + +.PHONY : battery/Battery.s + +# target to generate assembly for a file +battery/Battery.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/battery/Battery.cpp.s +.PHONY : battery/Battery.cpp.s + +brains/BrainFactory.o: brains/BrainFactory.cpp.o + +.PHONY : brains/BrainFactory.o + +# target to build an object file +brains/BrainFactory.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/BrainFactory.cpp.o +.PHONY : brains/BrainFactory.cpp.o + +brains/BrainFactory.i: brains/BrainFactory.cpp.i + +.PHONY : brains/BrainFactory.i + +# target to preprocess a source file +brains/BrainFactory.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/BrainFactory.cpp.i +.PHONY : brains/BrainFactory.cpp.i + +brains/BrainFactory.s: brains/BrainFactory.cpp.s + +.PHONY : brains/BrainFactory.s + +# target to generate assembly for a file +brains/BrainFactory.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/BrainFactory.cpp.s +.PHONY : brains/BrainFactory.cpp.s + +brains/DifferentialCPG.o: brains/DifferentialCPG.cpp.o + +.PHONY : brains/DifferentialCPG.o + +# target to build an object file +brains/DifferentialCPG.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/DifferentialCPG.cpp.o +.PHONY : brains/DifferentialCPG.cpp.o + +brains/DifferentialCPG.i: brains/DifferentialCPG.cpp.i + +.PHONY : brains/DifferentialCPG.i + +# target to preprocess a source file +brains/DifferentialCPG.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/DifferentialCPG.cpp.i +.PHONY : brains/DifferentialCPG.cpp.i + +brains/DifferentialCPG.s: brains/DifferentialCPG.cpp.s + +.PHONY : brains/DifferentialCPG.s + +# target to generate assembly for a file +brains/DifferentialCPG.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/DifferentialCPG.cpp.s +.PHONY : brains/DifferentialCPG.cpp.s + +brains/Evaluator.o: brains/Evaluator.cpp.o + +.PHONY : brains/Evaluator.o + +# target to build an object file +brains/Evaluator.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/Evaluator.cpp.o +.PHONY : brains/Evaluator.cpp.o + +brains/Evaluator.i: brains/Evaluator.cpp.i + +.PHONY : brains/Evaluator.i + +# target to preprocess a source file +brains/Evaluator.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/Evaluator.cpp.i +.PHONY : brains/Evaluator.cpp.i + +brains/Evaluator.s: brains/Evaluator.cpp.s + +.PHONY : brains/Evaluator.s + +# target to generate assembly for a file +brains/Evaluator.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/Evaluator.cpp.s +.PHONY : brains/Evaluator.cpp.s + +brains/NeuralNetwork.o: brains/NeuralNetwork.cpp.o + +.PHONY : brains/NeuralNetwork.o + +# target to build an object file +brains/NeuralNetwork.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/NeuralNetwork.cpp.o +.PHONY : brains/NeuralNetwork.cpp.o + +brains/NeuralNetwork.i: brains/NeuralNetwork.cpp.i + +.PHONY : brains/NeuralNetwork.i + +# target to preprocess a source file +brains/NeuralNetwork.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/NeuralNetwork.cpp.i +.PHONY : brains/NeuralNetwork.cpp.i + +brains/NeuralNetwork.s: brains/NeuralNetwork.cpp.s + +.PHONY : brains/NeuralNetwork.s + +# target to generate assembly for a file +brains/NeuralNetwork.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/NeuralNetwork.cpp.s +.PHONY : brains/NeuralNetwork.cpp.s + +brains/RLPower.o: brains/RLPower.cpp.o + +.PHONY : brains/RLPower.o + +# target to build an object file +brains/RLPower.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/RLPower.cpp.o +.PHONY : brains/RLPower.cpp.o + +brains/RLPower.i: brains/RLPower.cpp.i + +.PHONY : brains/RLPower.i + +# target to preprocess a source file +brains/RLPower.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/RLPower.cpp.i +.PHONY : brains/RLPower.cpp.i + +brains/RLPower.s: brains/RLPower.cpp.s + +.PHONY : brains/RLPower.s + +# target to generate assembly for a file +brains/RLPower.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/RLPower.cpp.s +.PHONY : brains/RLPower.cpp.s + +brains/ThymioBrain.o: brains/ThymioBrain.cpp.o + +.PHONY : brains/ThymioBrain.o + +# target to build an object file +brains/ThymioBrain.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/ThymioBrain.cpp.o +.PHONY : brains/ThymioBrain.cpp.o + +brains/ThymioBrain.i: brains/ThymioBrain.cpp.i + +.PHONY : brains/ThymioBrain.i + +# target to preprocess a source file +brains/ThymioBrain.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/ThymioBrain.cpp.i +.PHONY : brains/ThymioBrain.cpp.i + +brains/ThymioBrain.s: brains/ThymioBrain.cpp.s + +.PHONY : brains/ThymioBrain.s + +# target to generate assembly for a file +brains/ThymioBrain.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/ThymioBrain.cpp.s +.PHONY : brains/ThymioBrain.cpp.s + +motors/JointMotor.o: motors/JointMotor.cpp.o + +.PHONY : motors/JointMotor.o + +# target to build an object file +motors/JointMotor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/JointMotor.cpp.o +.PHONY : motors/JointMotor.cpp.o + +motors/JointMotor.i: motors/JointMotor.cpp.i + +.PHONY : motors/JointMotor.i + +# target to preprocess a source file +motors/JointMotor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/JointMotor.cpp.i +.PHONY : motors/JointMotor.cpp.i + +motors/JointMotor.s: motors/JointMotor.cpp.s + +.PHONY : motors/JointMotor.s + +# target to generate assembly for a file +motors/JointMotor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/JointMotor.cpp.s +.PHONY : motors/JointMotor.cpp.s + +motors/Motor.o: motors/Motor.cpp.o + +.PHONY : motors/Motor.o + +# target to build an object file +motors/Motor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/Motor.cpp.o +.PHONY : motors/Motor.cpp.o + +motors/Motor.i: motors/Motor.cpp.i + +.PHONY : motors/Motor.i + +# target to preprocess a source file +motors/Motor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/Motor.cpp.i +.PHONY : motors/Motor.cpp.i + +motors/Motor.s: motors/Motor.cpp.s + +.PHONY : motors/Motor.s + +# target to generate assembly for a file +motors/Motor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/Motor.cpp.s +.PHONY : motors/Motor.cpp.s + +motors/MotorFactory.o: motors/MotorFactory.cpp.o + +.PHONY : motors/MotorFactory.o + +# target to build an object file +motors/MotorFactory.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/MotorFactory.cpp.o +.PHONY : motors/MotorFactory.cpp.o + +motors/MotorFactory.i: motors/MotorFactory.cpp.i + +.PHONY : motors/MotorFactory.i + +# target to preprocess a source file +motors/MotorFactory.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/MotorFactory.cpp.i +.PHONY : motors/MotorFactory.cpp.i + +motors/MotorFactory.s: motors/MotorFactory.cpp.s + +.PHONY : motors/MotorFactory.s + +# target to generate assembly for a file +motors/MotorFactory.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/MotorFactory.cpp.s +.PHONY : motors/MotorFactory.cpp.s + +motors/PositionMotor.o: motors/PositionMotor.cpp.o + +.PHONY : motors/PositionMotor.o + +# target to build an object file +motors/PositionMotor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/PositionMotor.cpp.o +.PHONY : motors/PositionMotor.cpp.o + +motors/PositionMotor.i: motors/PositionMotor.cpp.i + +.PHONY : motors/PositionMotor.i + +# target to preprocess a source file +motors/PositionMotor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/PositionMotor.cpp.i +.PHONY : motors/PositionMotor.cpp.i + +motors/PositionMotor.s: motors/PositionMotor.cpp.s + +.PHONY : motors/PositionMotor.s + +# target to generate assembly for a file +motors/PositionMotor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/PositionMotor.cpp.s +.PHONY : motors/PositionMotor.cpp.s + +motors/VelocityMotor.o: motors/VelocityMotor.cpp.o + +.PHONY : motors/VelocityMotor.o + +# target to build an object file +motors/VelocityMotor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/VelocityMotor.cpp.o +.PHONY : motors/VelocityMotor.cpp.o + +motors/VelocityMotor.i: motors/VelocityMotor.cpp.i + +.PHONY : motors/VelocityMotor.i + +# target to preprocess a source file +motors/VelocityMotor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/VelocityMotor.cpp.i +.PHONY : motors/VelocityMotor.cpp.i + +motors/VelocityMotor.s: motors/VelocityMotor.cpp.s + +.PHONY : motors/VelocityMotor.s + +# target to generate assembly for a file +motors/VelocityMotor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/VelocityMotor.cpp.s +.PHONY : motors/VelocityMotor.cpp.s + +plugin/RobotController.o: plugin/RobotController.cpp.o + +.PHONY : plugin/RobotController.o + +# target to build an object file +plugin/RobotController.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/RobotController.cpp.o +.PHONY : plugin/RobotController.cpp.o + +plugin/RobotController.i: plugin/RobotController.cpp.i + +.PHONY : plugin/RobotController.i + +# target to preprocess a source file +plugin/RobotController.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/RobotController.cpp.i +.PHONY : plugin/RobotController.cpp.i + +plugin/RobotController.s: plugin/RobotController.cpp.s + +.PHONY : plugin/RobotController.s + +# target to generate assembly for a file +plugin/RobotController.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/RobotController.cpp.s +.PHONY : plugin/RobotController.cpp.s + +plugin/WorldController.o: plugin/WorldController.cpp.o + +.PHONY : plugin/WorldController.o + +# target to build an object file +plugin/WorldController.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/WorldController.cpp.o +.PHONY : plugin/WorldController.cpp.o + +plugin/WorldController.i: plugin/WorldController.cpp.i + +.PHONY : plugin/WorldController.i + +# target to preprocess a source file +plugin/WorldController.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/WorldController.cpp.i +.PHONY : plugin/WorldController.cpp.i + +plugin/WorldController.s: plugin/WorldController.cpp.s + +.PHONY : plugin/WorldController.s + +# target to generate assembly for a file +plugin/WorldController.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/WorldController.cpp.s +.PHONY : plugin/WorldController.cpp.s + +plugin/register_robot_plugin.o: plugin/register_robot_plugin.cpp.o + +.PHONY : plugin/register_robot_plugin.o + +# target to build an object file +plugin/register_robot_plugin.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/plugin/register_robot_plugin.cpp.o +.PHONY : plugin/register_robot_plugin.cpp.o + +plugin/register_robot_plugin.i: plugin/register_robot_plugin.cpp.i + +.PHONY : plugin/register_robot_plugin.i + +# target to preprocess a source file +plugin/register_robot_plugin.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/plugin/register_robot_plugin.cpp.i +.PHONY : plugin/register_robot_plugin.cpp.i + +plugin/register_robot_plugin.s: plugin/register_robot_plugin.cpp.s + +.PHONY : plugin/register_robot_plugin.s + +# target to generate assembly for a file +plugin/register_robot_plugin.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/plugin/register_robot_plugin.cpp.s +.PHONY : plugin/register_robot_plugin.cpp.s + +plugin/register_world_plugin.o: plugin/register_world_plugin.cpp.o + +.PHONY : plugin/register_world_plugin.o + +# target to build an object file +plugin/register_world_plugin.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/plugin/register_world_plugin.cpp.o +.PHONY : plugin/register_world_plugin.cpp.o + +plugin/register_world_plugin.i: plugin/register_world_plugin.cpp.i + +.PHONY : plugin/register_world_plugin.i + +# target to preprocess a source file +plugin/register_world_plugin.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/plugin/register_world_plugin.cpp.i +.PHONY : plugin/register_world_plugin.cpp.i + +plugin/register_world_plugin.s: plugin/register_world_plugin.cpp.s + +.PHONY : plugin/register_world_plugin.s + +# target to generate assembly for a file +plugin/register_world_plugin.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/plugin/register_world_plugin.cpp.s +.PHONY : plugin/register_world_plugin.cpp.s + +revolve/msgs/body.pb.o: revolve/msgs/body.pb.cc.o + +.PHONY : revolve/msgs/body.pb.o + +# target to build an object file +revolve/msgs/body.pb.cc.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/body.pb.cc.o +.PHONY : revolve/msgs/body.pb.cc.o + +revolve/msgs/body.pb.i: revolve/msgs/body.pb.cc.i + +.PHONY : revolve/msgs/body.pb.i + +# target to preprocess a source file +revolve/msgs/body.pb.cc.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/body.pb.cc.i +.PHONY : revolve/msgs/body.pb.cc.i + +revolve/msgs/body.pb.s: revolve/msgs/body.pb.cc.s + +.PHONY : revolve/msgs/body.pb.s + +# target to generate assembly for a file +revolve/msgs/body.pb.cc.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/body.pb.cc.s +.PHONY : revolve/msgs/body.pb.cc.s + +revolve/msgs/model_inserted.pb.o: revolve/msgs/model_inserted.pb.cc.o + +.PHONY : revolve/msgs/model_inserted.pb.o + +# target to build an object file +revolve/msgs/model_inserted.pb.cc.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/model_inserted.pb.cc.o +.PHONY : revolve/msgs/model_inserted.pb.cc.o + +revolve/msgs/model_inserted.pb.i: revolve/msgs/model_inserted.pb.cc.i + +.PHONY : revolve/msgs/model_inserted.pb.i + +# target to preprocess a source file +revolve/msgs/model_inserted.pb.cc.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/model_inserted.pb.cc.i +.PHONY : revolve/msgs/model_inserted.pb.cc.i + +revolve/msgs/model_inserted.pb.s: revolve/msgs/model_inserted.pb.cc.s + +.PHONY : revolve/msgs/model_inserted.pb.s + +# target to generate assembly for a file +revolve/msgs/model_inserted.pb.cc.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/model_inserted.pb.cc.s +.PHONY : revolve/msgs/model_inserted.pb.cc.s + +revolve/msgs/neural_net.pb.o: revolve/msgs/neural_net.pb.cc.o + +.PHONY : revolve/msgs/neural_net.pb.o + +# target to build an object file +revolve/msgs/neural_net.pb.cc.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/neural_net.pb.cc.o +.PHONY : revolve/msgs/neural_net.pb.cc.o + +revolve/msgs/neural_net.pb.i: revolve/msgs/neural_net.pb.cc.i + +.PHONY : revolve/msgs/neural_net.pb.i + +# target to preprocess a source file +revolve/msgs/neural_net.pb.cc.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/neural_net.pb.cc.i +.PHONY : revolve/msgs/neural_net.pb.cc.i + +revolve/msgs/neural_net.pb.s: revolve/msgs/neural_net.pb.cc.s + +.PHONY : revolve/msgs/neural_net.pb.s + +# target to generate assembly for a file +revolve/msgs/neural_net.pb.cc.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/neural_net.pb.cc.s +.PHONY : revolve/msgs/neural_net.pb.cc.s + +revolve/msgs/parameter.pb.o: revolve/msgs/parameter.pb.cc.o + +.PHONY : revolve/msgs/parameter.pb.o + +# target to build an object file +revolve/msgs/parameter.pb.cc.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/parameter.pb.cc.o +.PHONY : revolve/msgs/parameter.pb.cc.o + +revolve/msgs/parameter.pb.i: revolve/msgs/parameter.pb.cc.i + +.PHONY : revolve/msgs/parameter.pb.i + +# target to preprocess a source file +revolve/msgs/parameter.pb.cc.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/parameter.pb.cc.i +.PHONY : revolve/msgs/parameter.pb.cc.i + +revolve/msgs/parameter.pb.s: revolve/msgs/parameter.pb.cc.s + +.PHONY : revolve/msgs/parameter.pb.s + +# target to generate assembly for a file +revolve/msgs/parameter.pb.cc.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/parameter.pb.cc.s +.PHONY : revolve/msgs/parameter.pb.cc.s + +revolve/msgs/robot.pb.o: revolve/msgs/robot.pb.cc.o + +.PHONY : revolve/msgs/robot.pb.o + +# target to build an object file +revolve/msgs/robot.pb.cc.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot.pb.cc.o +.PHONY : revolve/msgs/robot.pb.cc.o + +revolve/msgs/robot.pb.i: revolve/msgs/robot.pb.cc.i + +.PHONY : revolve/msgs/robot.pb.i + +# target to preprocess a source file +revolve/msgs/robot.pb.cc.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot.pb.cc.i +.PHONY : revolve/msgs/robot.pb.cc.i + +revolve/msgs/robot.pb.s: revolve/msgs/robot.pb.cc.s + +.PHONY : revolve/msgs/robot.pb.s + +# target to generate assembly for a file +revolve/msgs/robot.pb.cc.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot.pb.cc.s +.PHONY : revolve/msgs/robot.pb.cc.s + +revolve/msgs/robot_states.pb.o: revolve/msgs/robot_states.pb.cc.o + +.PHONY : revolve/msgs/robot_states.pb.o + +# target to build an object file +revolve/msgs/robot_states.pb.cc.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot_states.pb.cc.o +.PHONY : revolve/msgs/robot_states.pb.cc.o + +revolve/msgs/robot_states.pb.i: revolve/msgs/robot_states.pb.cc.i + +.PHONY : revolve/msgs/robot_states.pb.i + +# target to preprocess a source file +revolve/msgs/robot_states.pb.cc.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot_states.pb.cc.i +.PHONY : revolve/msgs/robot_states.pb.cc.i + +revolve/msgs/robot_states.pb.s: revolve/msgs/robot_states.pb.cc.s + +.PHONY : revolve/msgs/robot_states.pb.s + +# target to generate assembly for a file +revolve/msgs/robot_states.pb.cc.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot_states.pb.cc.s +.PHONY : revolve/msgs/robot_states.pb.cc.s + +revolve/msgs/sdf_body_analyze.pb.o: revolve/msgs/sdf_body_analyze.pb.cc.o + +.PHONY : revolve/msgs/sdf_body_analyze.pb.o + +# target to build an object file +revolve/msgs/sdf_body_analyze.pb.cc.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/sdf_body_analyze.pb.cc.o +.PHONY : revolve/msgs/sdf_body_analyze.pb.cc.o + +revolve/msgs/sdf_body_analyze.pb.i: revolve/msgs/sdf_body_analyze.pb.cc.i + +.PHONY : revolve/msgs/sdf_body_analyze.pb.i + +# target to preprocess a source file +revolve/msgs/sdf_body_analyze.pb.cc.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/sdf_body_analyze.pb.cc.i +.PHONY : revolve/msgs/sdf_body_analyze.pb.cc.i + +revolve/msgs/sdf_body_analyze.pb.s: revolve/msgs/sdf_body_analyze.pb.cc.s + +.PHONY : revolve/msgs/sdf_body_analyze.pb.s + +# target to generate assembly for a file +revolve/msgs/sdf_body_analyze.pb.cc.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/sdf_body_analyze.pb.cc.s +.PHONY : revolve/msgs/sdf_body_analyze.pb.cc.s + +revolve/msgs/spline_policy.pb.o: revolve/msgs/spline_policy.pb.cc.o + +.PHONY : revolve/msgs/spline_policy.pb.o + +# target to build an object file +revolve/msgs/spline_policy.pb.cc.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/spline_policy.pb.cc.o +.PHONY : revolve/msgs/spline_policy.pb.cc.o + +revolve/msgs/spline_policy.pb.i: revolve/msgs/spline_policy.pb.cc.i + +.PHONY : revolve/msgs/spline_policy.pb.i + +# target to preprocess a source file +revolve/msgs/spline_policy.pb.cc.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/spline_policy.pb.cc.i +.PHONY : revolve/msgs/spline_policy.pb.cc.i + +revolve/msgs/spline_policy.pb.s: revolve/msgs/spline_policy.pb.cc.s + +.PHONY : revolve/msgs/spline_policy.pb.s + +# target to generate assembly for a file +revolve/msgs/spline_policy.pb.cc.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/spline_policy.pb.cc.s +.PHONY : revolve/msgs/spline_policy.pb.cc.s + +sensors/BatterySensor.o: sensors/BatterySensor.cpp.o + +.PHONY : sensors/BatterySensor.o + +# target to build an object file +sensors/BatterySensor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/BatterySensor.cpp.o +.PHONY : sensors/BatterySensor.cpp.o + +sensors/BatterySensor.i: sensors/BatterySensor.cpp.i + +.PHONY : sensors/BatterySensor.i + +# target to preprocess a source file +sensors/BatterySensor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/BatterySensor.cpp.i +.PHONY : sensors/BatterySensor.cpp.i + +sensors/BatterySensor.s: sensors/BatterySensor.cpp.s + +.PHONY : sensors/BatterySensor.s + +# target to generate assembly for a file +sensors/BatterySensor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/BatterySensor.cpp.s +.PHONY : sensors/BatterySensor.cpp.s + +sensors/ImuSensor.o: sensors/ImuSensor.cpp.o + +.PHONY : sensors/ImuSensor.o + +# target to build an object file +sensors/ImuSensor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/ImuSensor.cpp.o +.PHONY : sensors/ImuSensor.cpp.o + +sensors/ImuSensor.i: sensors/ImuSensor.cpp.i + +.PHONY : sensors/ImuSensor.i + +# target to preprocess a source file +sensors/ImuSensor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/ImuSensor.cpp.i +.PHONY : sensors/ImuSensor.cpp.i + +sensors/ImuSensor.s: sensors/ImuSensor.cpp.s + +.PHONY : sensors/ImuSensor.s + +# target to generate assembly for a file +sensors/ImuSensor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/ImuSensor.cpp.s +.PHONY : sensors/ImuSensor.cpp.s + +sensors/LightSensor.o: sensors/LightSensor.cpp.o + +.PHONY : sensors/LightSensor.o + +# target to build an object file +sensors/LightSensor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/LightSensor.cpp.o +.PHONY : sensors/LightSensor.cpp.o + +sensors/LightSensor.i: sensors/LightSensor.cpp.i + +.PHONY : sensors/LightSensor.i + +# target to preprocess a source file +sensors/LightSensor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/LightSensor.cpp.i +.PHONY : sensors/LightSensor.cpp.i + +sensors/LightSensor.s: sensors/LightSensor.cpp.s + +.PHONY : sensors/LightSensor.s + +# target to generate assembly for a file +sensors/LightSensor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/LightSensor.cpp.s +.PHONY : sensors/LightSensor.cpp.s + +sensors/PointIntensitySensor.o: sensors/PointIntensitySensor.cpp.o + +.PHONY : sensors/PointIntensitySensor.o + +# target to build an object file +sensors/PointIntensitySensor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/PointIntensitySensor.cpp.o +.PHONY : sensors/PointIntensitySensor.cpp.o + +sensors/PointIntensitySensor.i: sensors/PointIntensitySensor.cpp.i + +.PHONY : sensors/PointIntensitySensor.i + +# target to preprocess a source file +sensors/PointIntensitySensor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/PointIntensitySensor.cpp.i +.PHONY : sensors/PointIntensitySensor.cpp.i + +sensors/PointIntensitySensor.s: sensors/PointIntensitySensor.cpp.s + +.PHONY : sensors/PointIntensitySensor.s + +# target to generate assembly for a file +sensors/PointIntensitySensor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/PointIntensitySensor.cpp.s +.PHONY : sensors/PointIntensitySensor.cpp.s + +sensors/Sensor.o: sensors/Sensor.cpp.o + +.PHONY : sensors/Sensor.o + +# target to build an object file +sensors/Sensor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/Sensor.cpp.o +.PHONY : sensors/Sensor.cpp.o + +sensors/Sensor.i: sensors/Sensor.cpp.i + +.PHONY : sensors/Sensor.i + +# target to preprocess a source file +sensors/Sensor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/Sensor.cpp.i +.PHONY : sensors/Sensor.cpp.i + +sensors/Sensor.s: sensors/Sensor.cpp.s + +.PHONY : sensors/Sensor.s + +# target to generate assembly for a file +sensors/Sensor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/Sensor.cpp.s +.PHONY : sensors/Sensor.cpp.s + +sensors/SensorFactory.o: sensors/SensorFactory.cpp.o + +.PHONY : sensors/SensorFactory.o + +# target to build an object file +sensors/SensorFactory.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/SensorFactory.cpp.o +.PHONY : sensors/SensorFactory.cpp.o + +sensors/SensorFactory.i: sensors/SensorFactory.cpp.i + +.PHONY : sensors/SensorFactory.i + +# target to preprocess a source file +sensors/SensorFactory.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/SensorFactory.cpp.i +.PHONY : sensors/SensorFactory.cpp.i + +sensors/SensorFactory.s: sensors/SensorFactory.cpp.s + +.PHONY : sensors/SensorFactory.s + +# target to generate assembly for a file +sensors/SensorFactory.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/SensorFactory.cpp.s +.PHONY : sensors/SensorFactory.cpp.s + +sensors/TouchSensor.o: sensors/TouchSensor.cpp.o + +.PHONY : sensors/TouchSensor.o + +# target to build an object file +sensors/TouchSensor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/TouchSensor.cpp.o +.PHONY : sensors/TouchSensor.cpp.o + +sensors/TouchSensor.i: sensors/TouchSensor.cpp.i + +.PHONY : sensors/TouchSensor.i + +# target to preprocess a source file +sensors/TouchSensor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/TouchSensor.cpp.i +.PHONY : sensors/TouchSensor.cpp.i + +sensors/TouchSensor.s: sensors/TouchSensor.cpp.s + +.PHONY : sensors/TouchSensor.s + +# target to generate assembly for a file +sensors/TouchSensor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/TouchSensor.cpp.s +.PHONY : sensors/TouchSensor.cpp.s + +sensors/VirtualSensor.o: sensors/VirtualSensor.cpp.o + +.PHONY : sensors/VirtualSensor.o + +# target to build an object file +sensors/VirtualSensor.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/VirtualSensor.cpp.o +.PHONY : sensors/VirtualSensor.cpp.o + +sensors/VirtualSensor.i: sensors/VirtualSensor.cpp.i + +.PHONY : sensors/VirtualSensor.i + +# target to preprocess a source file +sensors/VirtualSensor.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/VirtualSensor.cpp.i +.PHONY : sensors/VirtualSensor.cpp.i + +sensors/VirtualSensor.s: sensors/VirtualSensor.cpp.s + +.PHONY : sensors/VirtualSensor.s + +# target to generate assembly for a file +sensors/VirtualSensor.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/VirtualSensor.cpp.s +.PHONY : sensors/VirtualSensor.cpp.s + +util/YamlBodyParser.o: util/YamlBodyParser.cpp.o + +.PHONY : util/YamlBodyParser.o + +# target to build an object file +util/YamlBodyParser.cpp.o: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/util/YamlBodyParser.cpp.o +.PHONY : util/YamlBodyParser.cpp.o + +util/YamlBodyParser.i: util/YamlBodyParser.cpp.i + +.PHONY : util/YamlBodyParser.i + +# target to preprocess a source file +util/YamlBodyParser.cpp.i: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/util/YamlBodyParser.cpp.i +.PHONY : util/YamlBodyParser.cpp.i + +util/YamlBodyParser.s: util/YamlBodyParser.cpp.s + +.PHONY : util/YamlBodyParser.s + +# target to generate assembly for a file +util/YamlBodyParser.cpp.s: + cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/util/YamlBodyParser.cpp.s +.PHONY : util/YamlBodyParser.cpp.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... list_install_components" + @echo "... rebuild_cache" + @echo "... install/strip" + @echo "... install" + @echo "... RobotControlPlugin" + @echo "... install/local" + @echo "... revolve-proto" + @echo "... WorldControlPlugin" + @echo "... revolve-gazebo" + @echo "... edit_cache" + @echo "... battery/Battery.o" + @echo "... battery/Battery.i" + @echo "... battery/Battery.s" + @echo "... brains/BrainFactory.o" + @echo "... brains/BrainFactory.i" + @echo "... brains/BrainFactory.s" + @echo "... brains/DifferentialCPG.o" + @echo "... brains/DifferentialCPG.i" + @echo "... brains/DifferentialCPG.s" + @echo "... brains/Evaluator.o" + @echo "... brains/Evaluator.i" + @echo "... brains/Evaluator.s" + @echo "... brains/NeuralNetwork.o" + @echo "... brains/NeuralNetwork.i" + @echo "... brains/NeuralNetwork.s" + @echo "... brains/RLPower.o" + @echo "... brains/RLPower.i" + @echo "... brains/RLPower.s" + @echo "... brains/ThymioBrain.o" + @echo "... brains/ThymioBrain.i" + @echo "... brains/ThymioBrain.s" + @echo "... motors/JointMotor.o" + @echo "... motors/JointMotor.i" + @echo "... motors/JointMotor.s" + @echo "... motors/Motor.o" + @echo "... motors/Motor.i" + @echo "... motors/Motor.s" + @echo "... motors/MotorFactory.o" + @echo "... motors/MotorFactory.i" + @echo "... motors/MotorFactory.s" + @echo "... motors/PositionMotor.o" + @echo "... motors/PositionMotor.i" + @echo "... motors/PositionMotor.s" + @echo "... motors/VelocityMotor.o" + @echo "... motors/VelocityMotor.i" + @echo "... motors/VelocityMotor.s" + @echo "... plugin/RobotController.o" + @echo "... plugin/RobotController.i" + @echo "... plugin/RobotController.s" + @echo "... plugin/WorldController.o" + @echo "... plugin/WorldController.i" + @echo "... plugin/WorldController.s" + @echo "... plugin/register_robot_plugin.o" + @echo "... plugin/register_robot_plugin.i" + @echo "... plugin/register_robot_plugin.s" + @echo "... plugin/register_world_plugin.o" + @echo "... plugin/register_world_plugin.i" + @echo "... plugin/register_world_plugin.s" + @echo "... revolve/msgs/body.pb.o" + @echo "... revolve/msgs/body.pb.i" + @echo "... revolve/msgs/body.pb.s" + @echo "... revolve/msgs/model_inserted.pb.o" + @echo "... revolve/msgs/model_inserted.pb.i" + @echo "... revolve/msgs/model_inserted.pb.s" + @echo "... revolve/msgs/neural_net.pb.o" + @echo "... revolve/msgs/neural_net.pb.i" + @echo "... revolve/msgs/neural_net.pb.s" + @echo "... revolve/msgs/parameter.pb.o" + @echo "... revolve/msgs/parameter.pb.i" + @echo "... revolve/msgs/parameter.pb.s" + @echo "... revolve/msgs/robot.pb.o" + @echo "... revolve/msgs/robot.pb.i" + @echo "... revolve/msgs/robot.pb.s" + @echo "... revolve/msgs/robot_states.pb.o" + @echo "... revolve/msgs/robot_states.pb.i" + @echo "... revolve/msgs/robot_states.pb.s" + @echo "... revolve/msgs/sdf_body_analyze.pb.o" + @echo "... revolve/msgs/sdf_body_analyze.pb.i" + @echo "... revolve/msgs/sdf_body_analyze.pb.s" + @echo "... revolve/msgs/spline_policy.pb.o" + @echo "... revolve/msgs/spline_policy.pb.i" + @echo "... revolve/msgs/spline_policy.pb.s" + @echo "... sensors/BatterySensor.o" + @echo "... sensors/BatterySensor.i" + @echo "... sensors/BatterySensor.s" + @echo "... sensors/ImuSensor.o" + @echo "... sensors/ImuSensor.i" + @echo "... sensors/ImuSensor.s" + @echo "... sensors/LightSensor.o" + @echo "... sensors/LightSensor.i" + @echo "... sensors/LightSensor.s" + @echo "... sensors/PointIntensitySensor.o" + @echo "... sensors/PointIntensitySensor.i" + @echo "... sensors/PointIntensitySensor.s" + @echo "... sensors/Sensor.o" + @echo "... sensors/Sensor.i" + @echo "... sensors/Sensor.s" + @echo "... sensors/SensorFactory.o" + @echo "... sensors/SensorFactory.i" + @echo "... sensors/SensorFactory.s" + @echo "... sensors/TouchSensor.o" + @echo "... sensors/TouchSensor.i" + @echo "... sensors/TouchSensor.s" + @echo "... sensors/VirtualSensor.o" + @echo "... sensors/VirtualSensor.i" + @echo "... sensors/VirtualSensor.s" + @echo "... util/YamlBodyParser.o" + @echo "... util/YamlBodyParser.i" + @echo "... util/YamlBodyParser.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/cpprevolve/revolve/gazebo/battery/Battery.cpp b/cpprevolve/revolve/gazebo/battery/Battery.cpp index 4712307d29..c6039283c6 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.cpp +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -7,7 +7,7 @@ using namespace revolve::gazebo; Battery::Battery(double initial_charge) - : initial_charge(initial_charge), current_charge(initial_charge), watts_used(0) + : initial_charge(initial_charge), current_charge(initial_charge), watts_used(0), time_init(std::to_string(time(0))) {} void Battery::Update(double global_time, double delta_time) @@ -16,11 +16,11 @@ void Battery::Update(double global_time, double delta_time) // 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; + sum += consumer.second; // TODO add constant so its linear } this->current_charge += sum * delta_time; // charge is measured in joules std::ofstream b_info_file; - b_info_file.open("/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/battery/battery_info_spider9_0.txt", std::ios_base::app); + b_info_file.open("/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/battery/data/babyA/battery_info_" + this->time_init + ".txt", std::ios_base::app); b_info_file << global_time << " " << sum << " " << current_charge << std::endl; -// std::cout << this->watts_used<< std::endl; +// std::cout << this->time_init<< std::endl; } diff --git a/cpprevolve/revolve/gazebo/battery/Battery.h b/cpprevolve/revolve/gazebo/battery/Battery.h index 7acc082fba..0e053db2bb 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.h +++ b/cpprevolve/revolve/gazebo/battery/Battery.h @@ -33,6 +33,9 @@ class Battery : public ::gazebo::common::Battery /// \brief amount of watts used for all servos at a time double watts_used; + /// \brief the time of initiation (for creating data files of battery delete later) + std::string time_init; + friend class Evaluator; }; diff --git a/cpprevolve/revolve/gazebo/battery/battery_chart.py b/cpprevolve/revolve/gazebo/battery/battery_chart.py index a12ee06b66..5a8a984068 100644 --- a/cpprevolve/revolve/gazebo/battery/battery_chart.py +++ b/cpprevolve/revolve/gazebo/battery/battery_chart.py @@ -57,10 +57,10 @@ def main(): watts_used.append(float(data[1])) current_charge.append(float(data[2])) if float(data[0]) > 30 and draw30: - # draw_graphs(global_time, watts_used, current_charge) + draw_graphs(global_time, watts_used, current_charge) draw30 = False draw_graph(global_time, watts_used) - # draw_graphs(global_time, watts_used, current_charge) + draw_graphs(global_time, watts_used, current_charge) if __name__ == "__main__": main() \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 84a2ad00e7..70bf3073b0 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -375,7 +375,8 @@ DifferentialCPG::~DifferentialCPG() */ struct DifferentialCPG::evaluation_function{ // Number of input dimension (samples.size()) - BO_PARAM(size_t, dim_in, 18); + //spider9:18,spider13:26,spider17:34,gecko7:13,gecko12:23,gecko17:33,babyA:16,babyB:22,babyC:32,one+:12 + BO_PARAM(size_t, dim_in, 16); // STEP 2 // number of dimensions of the fitness BO_PARAM(size_t, dim_out, 1); diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp index 0e87d4d79e..cce5deb450 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp @@ -52,7 +52,7 @@ Evaluator::Evaluator(std::shared_ptr<::revolve::gazebo::Battery> battery, this->current_position_.Reset(); this->previous_position_.Reset(); this->start_position_.Reset(); - this->locomotion_type = "directed"; // {directed, gait, battery} + this->locomotion_type = "directed"; // {directed, gait, battery} // STEP 3 this->path_length = 0.0; } diff --git a/cpprevolve/revolve/gazebo/cmake_install.cmake b/cpprevolve/revolve/gazebo/cmake_install.cmake new file mode 100644 index 0000000000..3eee625be8 --- /dev/null +++ b/cpprevolve/revolve/gazebo/cmake_install.cmake @@ -0,0 +1,54 @@ +# Install script for directory: /Users/roy/projects/revolve/cpprevolve/revolve/gazebo + +# Set the install prefix +if(NOT DEFINED CMAKE_INSTALL_PREFIX) + set(CMAKE_INSTALL_PREFIX "/usr/local") +endif() +string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + if(BUILD_TYPE) + string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + else() + set(CMAKE_INSTALL_CONFIG_NAME "Debug") + endif() + message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +endif() + +# Set the component getting installed. +if(NOT CMAKE_INSTALL_COMPONENT) + if(COMPONENT) + message(STATUS "Install component: \"${COMPONENT}\"") + set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + else() + set(CMAKE_INSTALL_COMPONENT) + endif() +endif() + +# Is this installation the result of a crosscompile? +if(NOT DEFINED CMAKE_CROSSCOMPILING) + set(CMAKE_CROSSCOMPILING "FALSE") +endif() + +if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) + file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/lib" TYPE STATIC_LIBRARY FILES "/Users/roy/projects/revolve/build/lib/librevolve-proto.a") + if(EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-proto.a" AND + NOT IS_SYMLINK "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-proto.a") + execute_process(COMMAND "/Library/Developer/CommandLineTools/usr/bin/ranlib" "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-proto.a") + endif() +endif() + +if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) + file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/lib" TYPE STATIC_LIBRARY FILES "/Users/roy/projects/revolve/build/lib/librevolve-gazebo.a") + if(EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-gazebo.a" AND + NOT IS_SYMLINK "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-gazebo.a") + execute_process(COMMAND "/Library/Developer/CommandLineTools/usr/bin/ranlib" "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-gazebo.a") + endif() +endif() + +if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) + file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/include" TYPE DIRECTORY FILES "/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/revolve" FILES_MATCHING REGEX "/[^/]*\\.h$") +endif() + diff --git a/experiments/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index 7a4717c157..f500425cee 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -1,3 +1,10 @@ +""" + experiments = [ + {'range_ub': 1.0, 'signal_factor_all': 1.0}, + {'range_ub': 1.0, 'signal_factor_all': 3.0}, + {'range_ub': 4.5, 'signal_factor_all': 3.0} + ] +""" from sys import platform import matplotlib if platform == "darwin": @@ -12,63 +19,39 @@ # Parameters -targeted = True -n_objects = 10 -min_lines = 0 # This is a bit different with targeted flow -run_gazebo = True -n_runs = 1 -run_factor = 1 -n_jobs = 1 +min_lines = 110 +run_gazebo = False +n_runs = 10 # Naar 20 +n_jobs = 4 my_yaml_path = "experiments/bo_learner/yaml/" -yaml_model = "spider9.yaml" +yaml_model = "babyA.yaml" manager = "experiments/bo_learner/manager.py" -python_interpreter = ".venv/bin/python3" -start_port = 12000 +python_interpreter = "~/projects/revolve/.venv/bin/python3" search_space = { + # 'load_brain': ["/Users/lan/projects/revolve/output/cpg_bo/one/main_1560413639/0/0/best_brain.txt"], + 'evaluation_rate': [60], + 'init_method': ["LHS"], 'verbose': [1], - # 'for_slower_amplitude_factor': [3.0], - # 'load_brain': ["'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378190.71/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389414.36/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378245.76/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378287.77/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378160.67/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389312.53/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378225.74/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389379.72/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389361.89/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389338.31/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389234.1/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378190.49/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378165.66/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389121.16/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389123.4/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378444.49/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378241.33/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389526.78/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389088.1/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389193.31/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389159.72/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378109.28/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389284.85/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378216.32/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389419.81/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378267.39/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378104.54/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560378235.74/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389173.43/best_brain1501.txt'", - # "'/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1560378054-1-gecko17/0/1560389430.53/best_brain1501.txt'", - # ] + 'kernel_l': [0.1], + 'acqui_ucb_alpha': [1.0], + 'n_learning_iterations': [100], + 'n_init_samples': [20], + # 'kernel_l': [0.2], + # 'acqui_ucb_alpha': [3.0], + # 'n_learning_iterations': [950], + # 'n_init_samples': [20], } print(search_space) # You don't have to change this my_sub_directory = "yaml_temp/" output_path = "output/cpg_bo/main_" + str(round(time.time())) + "/" +start_port = 11000 # STEP 6 finished = False # Make in revolve/build to allow runs from terminal -os.system('mkdir build9 && cd build9 && cmake /Users/roy/projects/revolve -DCMAKE_BUILD_TYPE="Release"') -os.system("make -j4") +# os.system('mkdir build9 && cd build9 && cmake ~/projects/revolve/ -DCMAKE_BUILD_TYPE="Release" -DLOCAL_GAZEBO_DIR=""') +# os.system("make -j4") def change_parameters(original_file, parameters): # Iterate over dictionary elements @@ -113,7 +96,7 @@ def create_yamls(yaml_path, model, sub_directory, experiments): def run(i, sub_directory, model, params): # Sleepy time when starting up to save gazebo from misery if i < n_jobs: - time.sleep(3.5*i) + time.sleep(5*i) else: print("Todo: Make sure you are leaving 2 seconds in between firing " "gazebos") @@ -149,7 +132,7 @@ def run(i, sub_directory, model, params): if not run_gazebo: py_command = python_interpreter + \ " ./revolve.py" + \ - - " --manager " + manager + \ + " --manager " + manager + \ " --world-address " + world_address + \ " --robot-yaml " + yaml_model else: @@ -169,15 +152,43 @@ def run(i, sub_directory, model, params): # Get permutations keys, values = zip(*search_space.items()) experiments = [dict(zip(keys, v)) for v in itertools.product(*values)] + + # PASTE THE EXPERIMENTS HERE, IN THE FORMAT SHOWN BELOW + experiments = [ + {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0} # STEP 4 + + # # BASE RUN + # {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # {'init_method': "LHS", 'kernel_l': 0.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # {'init_method': "LHS", 'kernel_l': 1.0, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # {'init_method': "LHS", 'kernel_l': 1.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.1, 'acqui_ucb_alpha': 0.5}, + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.2, 'acqui_ucb_alpha': 0.5}, + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.5, 'acqui_ucb_alpha': 0.5}, + # # BASE RUN + # + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.1}, + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.2}, + # # BASE RUN + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.0}, + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.5}, + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 2.0}, + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0}, + # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 4.0}, + # + # {'init_method': "RS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # BASE RUN + ] + # 'kernel_l': [0.02, 0.05, 0.1, 0.2], + # 'acqui_ucb_alpha': [0.1, 0.3, 0.5, 1.0], unique_experiments = experiments n_unique_experiments = len(experiments) # Get id's on the permutations for ix, my_dict in enumerate(experiments): my_dict["id"] = ix - - # Create a list with parameters to iterate over - experiments *= n_runs + experiments *=n_runs # Save to yaml files create_yamls(yaml_path=my_yaml_path, @@ -217,19 +228,12 @@ def run(i, sub_directory, model, params): runs_succesful[str(e.split("/")[-2])] = 0 for my_run in runs: - if not targeted: - if os.path.isfile(my_run + "fitnesses.txt"): - n_lines = len([(line.rstrip('\n')) for line in open(my_run + "fitnesses.txt")]) - - # In case we had a succesful run - if n_lines > min_lines: - runs_succesful[str(e.split("/")[-2])] += 1 - else: - if os.path.isfile(my_run + "speed_to_object.txt"): - n_lines = len([(line.rstrip('\n')) for line in open(my_run + "speed_to_object.txt")]) - if n_lines >= n_objects: - runs_succesful[str(e.split("/")[-2])] += 1 + if os.path.isfile(my_run + "fitnesses.txt"): + n_lines = len([(line.rstrip('\n')) for line in open(my_run + "fitnesses.txt")]) + # In case we had a succesful run + if n_lines > min_lines: + runs_succesful[str(e.split("/")[-2])] += 1 to_run = {} for key, val in runs_succesful.items(): diff --git a/experiments/bo_learner/yaml/babyA.yaml b/experiments/bo_learner/yaml/babyA.yaml index e1420ea4e7..7b29507ff8 100644 --- a/experiments/bo_learner/yaml/babyA.yaml +++ b/experiments/bo_learner/yaml/babyA.yaml @@ -167,7 +167,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -184,10 +184,10 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: 1 startup_time: 0 diff --git a/experiments/bo_learner/yaml/spider9.yaml b/experiments/bo_learner/yaml/spider9.yaml index 1df00f7444..52629eb41f 100644 --- a/experiments/bo_learner/yaml/spider9.yaml +++ b/experiments/bo_learner/yaml/spider9.yaml @@ -159,7 +159,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -176,7 +176,7 @@ brain: meta: robot_size: 18 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml b/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml index 4f3e523f9a..dc5b12e4b0 100644 --- a/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml +++ b/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml @@ -180,6 +180,6 @@ brain: n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 - output_directory: "output/cpg_bo/main_1563198613/0/1563198803.71/" + output_directory: "output/cpg_bo/main_1563286374/0/1563286549.13/" verbose: 1 startup_time: 0 From 935aa80af9c59cc7156f8147863bd0d388e3d496 Mon Sep 17 00:00:00 2001 From: roy Date: Fri, 19 Jul 2019 11:56:17 +0200 Subject: [PATCH 05/19] fixed directory for battery data --- cpprevolve/revolve/gazebo/battery/Battery.cpp | 3 +- .../revolve/gazebo/brains/DifferentialCPG.cpp | 2 +- experiments/bo_learner/GridSearch.py | 2 +- experiments/bo_learner/yaml/babyB.yaml | 6 +- experiments/bo_learner/yaml/babyC.yaml | 6 +- experiments/bo_learner/yaml/gecko12.yaml | 6 +- experiments/bo_learner/yaml/gecko17.yaml | 6 +- experiments/bo_learner/yaml/gecko7.yaml | 6 +- experiments/bo_learner/yaml/spider13.yaml | 6 +- experiments/bo_learner/yaml/spider17.yaml | 6 +- .../yaml/yaml_temp/spider9-0-0.yaml | 185 ------------------ .../bo_learner/yaml/yaml_temp/spider9-0.yaml | 185 ------------------ 12 files changed, 25 insertions(+), 394 deletions(-) delete mode 100644 experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml delete mode 100644 experiments/bo_learner/yaml/yaml_temp/spider9-0.yaml diff --git a/cpprevolve/revolve/gazebo/battery/Battery.cpp b/cpprevolve/revolve/gazebo/battery/Battery.cpp index c6039283c6..c4d452b3da 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.cpp +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -20,7 +20,8 @@ void Battery::Update(double global_time, double delta_time) } this->current_charge += sum * delta_time; // charge is measured in joules std::ofstream b_info_file; - b_info_file.open("/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/battery/data/babyA/battery_info_" + this->time_init + ".txt", std::ios_base::app); + b_info_file.open("cpprevolve/revolve/gazebo/battery/data/babyC/battery_info_" + this->time_init + ".txt", std::ios_base::app); + b_info_file << global_time << " " << sum << " " << current_charge << std::endl; // std::cout << this->time_init<< std::endl; } diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 70bf3073b0..d8059f348e 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -376,7 +376,7 @@ DifferentialCPG::~DifferentialCPG() struct DifferentialCPG::evaluation_function{ // Number of input dimension (samples.size()) //spider9:18,spider13:26,spider17:34,gecko7:13,gecko12:23,gecko17:33,babyA:16,babyB:22,babyC:32,one+:12 - BO_PARAM(size_t, dim_in, 16); // STEP 2 + BO_PARAM(size_t, dim_in, 32); // STEP 2 // number of dimensions of the fitness BO_PARAM(size_t, dim_out, 1); diff --git a/experiments/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index f500425cee..e5dd9789cd 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -24,7 +24,7 @@ n_runs = 10 # Naar 20 n_jobs = 4 my_yaml_path = "experiments/bo_learner/yaml/" -yaml_model = "babyA.yaml" +yaml_model = "babyC.yaml" manager = "experiments/bo_learner/manager.py" python_interpreter = "~/projects/revolve/.venv/bin/python3" search_space = { diff --git a/experiments/bo_learner/yaml/babyB.yaml b/experiments/bo_learner/yaml/babyB.yaml index 1144e9f64f..90883533b5 100644 --- a/experiments/bo_learner/yaml/babyB.yaml +++ b/experiments/bo_learner/yaml/babyB.yaml @@ -199,7 +199,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -213,10 +213,10 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: 1 startup_time: 0 diff --git a/experiments/bo_learner/yaml/babyC.yaml b/experiments/bo_learner/yaml/babyC.yaml index 8448aa9939..55ff08464f 100644 --- a/experiments/bo_learner/yaml/babyC.yaml +++ b/experiments/bo_learner/yaml/babyC.yaml @@ -313,7 +313,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -330,10 +330,10 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: 1 startup_time: 0 diff --git a/experiments/bo_learner/yaml/gecko12.yaml b/experiments/bo_learner/yaml/gecko12.yaml index bb6dfaaeab..7c8ab72a74 100644 --- a/experiments/bo_learner/yaml/gecko12.yaml +++ b/experiments/bo_learner/yaml/gecko12.yaml @@ -230,7 +230,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -244,10 +244,10 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: 1 startup_time: 0 diff --git a/experiments/bo_learner/yaml/gecko17.yaml b/experiments/bo_learner/yaml/gecko17.yaml index 1c32cf0ede..263564ccae 100644 --- a/experiments/bo_learner/yaml/gecko17.yaml +++ b/experiments/bo_learner/yaml/gecko17.yaml @@ -322,7 +322,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -336,10 +336,10 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: 1 startup_time: 0 diff --git a/experiments/bo_learner/yaml/gecko7.yaml b/experiments/bo_learner/yaml/gecko7.yaml index 00b3eee25e..e58dfe0bcc 100644 --- a/experiments/bo_learner/yaml/gecko7.yaml +++ b/experiments/bo_learner/yaml/gecko7.yaml @@ -139,7 +139,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -153,10 +153,10 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: 1 startup_time: 0 diff --git a/experiments/bo_learner/yaml/spider13.yaml b/experiments/bo_learner/yaml/spider13.yaml index 5fed980f4e..2244f9f1f7 100644 --- a/experiments/bo_learner/yaml/spider13.yaml +++ b/experiments/bo_learner/yaml/spider13.yaml @@ -235,7 +235,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -249,10 +249,10 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: 1 startup_time: 0 diff --git a/experiments/bo_learner/yaml/spider17.yaml b/experiments/bo_learner/yaml/spider17.yaml index 2cdae21c26..2c292422ed 100644 --- a/experiments/bo_learner/yaml/spider17.yaml +++ b/experiments/bo_learner/yaml/spider17.yaml @@ -307,7 +307,7 @@ brain: range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 50 + n_init_samples: 20 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -321,10 +321,10 @@ brain: meta: robot_size: 34 run_analytics: "true" - n_learning_iterations: 1450 + n_learning_iterations: 100 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 0 + verbose: 1 startup_time: 0 diff --git a/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml b/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml deleted file mode 100644 index dc5b12e4b0..0000000000 --- a/experiments/bo_learner/yaml/yaml_temp/spider9-0-0.yaml +++ /dev/null @@ -1,185 +0,0 @@ ---- -id: example_spider -body: - id : Core - type : Core - params: - red: 0.04 - green: 0.26 - blue: 0.72 - children : - 0: - id : Leg00Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - orientation : 90 - children : - 1: - id : Leg00 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : -90 - children : - 1: - id : Leg01Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - children : - 1: - id : Leg01 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : 0 - 1: - id : Leg10Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - orientation : 90 - children : - 1: - id : Leg10 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : -90 - children : - 1: - id : Leg11Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - children : - 1: - id : Leg11 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : 0 - 2: - id : Leg20Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - orientation : 90 - children : - 1: - id : Leg20 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : -90 - children : - 1: - id : Leg21Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - children : - 1: - id : Leg21 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : 0 - 3: - id : Leg30Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - orientation : 90 - children : - 1: - id : Leg30 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : -90 - children : - 1: - id : Leg31Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - children : - 1: - id : Leg31 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : 0 -brain: - type: bo-cpg - controller: - use_frame_of_reference: 0 - load_brain: "" - reset_neuron_state_bool: "true" - reset_neuron_random: "false" - abs_output_bound: 1.0 - signal_factor_all: 4.0 - signal_factor_mid: 2.5 - signal_factor_left_right: 2.5 - range_lb: -1.0 - range_ub: 1.5 - init_neuron_state: 0.707 - learner: - n_init_samples: 50 - init_method: "LHS" - kernel_noise: 0.00000001 - kernel_optimize_noise: "false" - kernel_sigma_sq: 0.222 - kernel_l: 0.55 - kernel_squared_exp_ard_k: 4 - acqui_gpucb_delta: 0.5 - acqui_ucb_alpha: 0.44 - acqui_ei_jitter: 0 - acquisition_function: "UCB" - gaussian_step_size: 0.3 - covrate: 0.4 - mutrate: 0.6 - meta: - robot_size: 18 - run_analytics: "true" - n_learning_iterations: 1450 - n_cooldown_iterations: 1 - reset_robot_position: "false" - evaluation_rate: 60 - output_directory: "output/cpg_bo/main_1563286374/0/1563286549.13/" - verbose: 1 - startup_time: 0 diff --git a/experiments/bo_learner/yaml/yaml_temp/spider9-0.yaml b/experiments/bo_learner/yaml/yaml_temp/spider9-0.yaml deleted file mode 100644 index 9344de9abd..0000000000 --- a/experiments/bo_learner/yaml/yaml_temp/spider9-0.yaml +++ /dev/null @@ -1,185 +0,0 @@ ---- -id: example_spider -body: - id : Core - type : Core - params: - red: 0.04 - green: 0.26 - blue: 0.72 - children : - 0: - id : Leg00Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - orientation : 90 - children : - 1: - id : Leg00 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : -90 - children : - 1: - id : Leg01Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - children : - 1: - id : Leg01 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : 0 - 1: - id : Leg10Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - orientation : 90 - children : - 1: - id : Leg10 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : -90 - children : - 1: - id : Leg11Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - children : - 1: - id : Leg11 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : 0 - 2: - id : Leg20Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - orientation : 90 - children : - 1: - id : Leg20 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : -90 - children : - 1: - id : Leg21Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - children : - 1: - id : Leg21 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : 0 - 3: - id : Leg30Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - orientation : 90 - children : - 1: - id : Leg30 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : -90 - children : - 1: - id : Leg31Joint - type : ActiveHinge - params: - red: 0.98 - green: 0.98 - blue: 0.98 - children : - 1: - id : Leg31 - type : FixedBrick - params: - red: 0.04 - green: 0.26 - blue: 0.72 - orientation : 0 -brain: - type: bo-cpg - controller: - use_frame_of_reference: 0 - load_brain: "" - reset_neuron_state_bool: "true" - reset_neuron_random: "false" - abs_output_bound: 1.0 - signal_factor_all: 4.0 - signal_factor_mid: 2.5 - signal_factor_left_right: 2.5 - range_lb: -1.0 - range_ub: 1.5 - init_neuron_state: 0.707 - learner: - n_init_samples: 50 - init_method: "LHS" - kernel_noise: 0.00000001 - kernel_optimize_noise: "false" - kernel_sigma_sq: 0.222 - kernel_l: 0.55 - kernel_squared_exp_ard_k: 4 - acqui_gpucb_delta: 0.5 - acqui_ucb_alpha: 0.44 - acqui_ei_jitter: 0 - acquisition_function: "UCB" - gaussian_step_size: 0.3 - covrate: 0.4 - mutrate: 0.6 - meta: - robot_size: 18 - run_analytics: "true" - n_learning_iterations: 1450 - n_cooldown_iterations: 1 - reset_robot_position: "false" - evaluation_rate: 60 - output_directory: "" - verbose: 1 - startup_time: 0 From 08f83cd6ef5041a5cd067994662cba63ebd92e2c Mon Sep 17 00:00:00 2001 From: roy Date: Mon, 22 Jul 2019 17:04:42 +0200 Subject: [PATCH 06/19] fixed directory for battery data and speed data --- cpprevolve/revolve/gazebo/battery/Battery.cpp | 3 +- cpprevolve/revolve/gazebo/battery/Battery.h | 1 + .../revolve/gazebo/battery/battery_chart.py | 174 +++++++++++++++--- .../gazebo/battery/data/babyA/delete_this.txt | 0 .../gazebo/battery/data/babyB/delete_this.txt | 0 .../gazebo/battery/data/babyC/delete_this.txt | 0 .../battery/data/gecko12/delete_this.txt | 0 .../battery/data/gecko17/delete_this.txt | 0 .../battery/data/gecko7/delete_this.txt | 0 .../battery/data/spider13/delete_this.txt | 0 .../battery/data/spider17/delete_this.txt | 0 .../battery/data/spider9/delete_this.txt | 0 .../revolve/gazebo/brains/DifferentialCPG.cpp | 30 ++- .../revolve/gazebo/brains/Evaluator.cpp | 9 +- experiments/bo_learner/GridSearch.py | 4 +- 15 files changed, 181 insertions(+), 40 deletions(-) delete mode 100644 cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt diff --git a/cpprevolve/revolve/gazebo/battery/Battery.cpp b/cpprevolve/revolve/gazebo/battery/Battery.cpp index c4d452b3da..0f947ba47f 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.cpp +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -20,7 +20,8 @@ void Battery::Update(double global_time, double delta_time) } this->current_charge += sum * delta_time; // charge is measured in joules std::ofstream b_info_file; - b_info_file.open("cpprevolve/revolve/gazebo/battery/data/babyC/battery_info_" + this->time_init + ".txt", std::ios_base::app); + // CHANGETHIS + b_info_file.open("cpprevolve/revolve/gazebo/battery/data/babyA/battery_info_" + this->time_init + ".txt", std::ios_base::app); b_info_file << global_time << " " << sum << " " << current_charge << std::endl; // std::cout << this->time_init<< std::endl; diff --git a/cpprevolve/revolve/gazebo/battery/Battery.h b/cpprevolve/revolve/gazebo/battery/Battery.h index 0e053db2bb..017b4dfc99 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.h +++ b/cpprevolve/revolve/gazebo/battery/Battery.h @@ -37,6 +37,7 @@ class Battery : public ::gazebo::common::Battery std::string time_init; friend class Evaluator; + friend class DifferentialCPG; }; } diff --git a/cpprevolve/revolve/gazebo/battery/battery_chart.py b/cpprevolve/revolve/gazebo/battery/battery_chart.py index 5a8a984068..d053525ac9 100644 --- a/cpprevolve/revolve/gazebo/battery/battery_chart.py +++ b/cpprevolve/revolve/gazebo/battery/battery_chart.py @@ -1,17 +1,72 @@ import matplotlib import matplotlib.pyplot as plt import numpy as np - +import pandas as pd +from os import listdir +from os.path import isfile, join from scipy.ndimage.filters import gaussian_filter1d -''' -MacOSX -TkAgg -TkCairo -''' +def nice_chart(col_name, do_log=False, do_exp=False, axs=None, col_normalize=None): + n = 3 + viridis = matplotlib.cm.get_cmap('viridis', n) + colormap = viridis(np.linspace(0, 1, n)) + if axs is None: + fig, axs = plt.subplots(1,1, figsize=(8,8)) + + title = col_name + if col_normalize is not None: + title = col_name + ' / ' + col_normalize + if do_log: + title = title + " (log)" + elif do_exp: + title = title + " (exp)" + + + axs.set_title(title) + + for_list = [ + (0, 'direct', direct, axs, colormap[0]), + (1, 'lsystem', lsystem, axs, colormap[1]), + (2, 'cppn', cppn, axs, colormap[2]), + ] + + for i, title, data, ax, color in for_list: + values = [] + for name,group in data.groupby('gen'): + line_values = group[col_name] + if col_normalize is not None: + line_values = line_values / group[col_normalize] + ax.set_ylim((0,1)) + if do_log: + line_values = np.log(line_values) + elif do_exp: + line_values = np.exp(line_values) + + values.append([np.average(line_values)] + np.percentile(line_values, [0, 25, 50, 75, 100]).tolist()) + values = pd.DataFrame(data=values, columns=['avg','0','25','50','75','100']) + + x = np.arange(600) + #ax.set_title(title) + ax.plot(values['avg'], linewidth=2, color=color, label=title) + ax.fill_between(x, values['25'], values['75'], alpha=0.4, color=color) + + offset= i*10 + x = np.arange(offset, 599, 60) + yerr_top = [] + yerr_bottom = [] + yerr_interval = [] + for i in range(len(x)): + yerr_top.append(values['avg'][x[i]] - values['100'][x[i]]) + yerr_bottom.append(values['0'][x[i]] - values['avg'][x[i]]) + yerr_interval.append(values['avg'][x[i]]) + ax.errorbar(x, yerr_interval, yerr=[yerr_top, yerr_bottom], alpha=0.4, capsize=2, color=color); + + axs.legend(fontsize=12, loc="lower right", bbox_to_anchor=[1, 0], + ncol=2, shadow=True, fancybox=True) + def average(n, lst): res = [] @@ -20,47 +75,108 @@ def average(n, lst): return res -def draw_graph(x, y): - plt.title('Battery Plot') +def draw_graph(name, x, y): + plt.title('Watts Used Plot ' + name) # plt.plot(x, y) plt.plot(x, gaussian_filter1d(y, sigma=100)) plt.plot(x, gaussian_filter1d(y, sigma=300)) plt.plot(x, gaussian_filter1d(y, sigma=500)) plt.plot(x, gaussian_filter1d(y, sigma=1000)) - - plt.ylabel('energy used at instance (joules)') + # plt.vlines(range(0,int(x[-1]),60), -0.13, -0.225, colors='k', linestyles='solid', label='') + plt.ylabel('amount of watts used on current time\'s update') plt.xlabel('time (s)') plt.show() -def draw_graphs(x, y1, y2): - plt.title('Battery Plot') +def draw_graphs(name, x, y1, y2): + plt.title('Current Charge Plot ' + name) plt.subplot(2, 1, 1) - plt.plot(x, y1) + # plt.vlines(range(0,int(x[-1]),60), -0.13, -0.17, colors='k', linestyles='solid', label='') + plt.plot(x, gaussian_filter1d(y1, sigma=100)) + plt.plot(x, gaussian_filter1d(y1, sigma=300)) + plt.plot(x, gaussian_filter1d(y1, sigma=500)) + plt.plot(x, gaussian_filter1d(y1, sigma=1000)) plt.ylabel('energy used at instance (joules)') - plt.subplot(2, 1, 2) plt.plot(x, y2) + + # plt.vlines(range(0,int(x[-1]),60), 0, y2[-1], colors='k', linestyles='solid', label='') plt.xlabel('time (s)') plt.ylabel('total energy used (joules)') plt.show() def main(): # matplotlib.use('MacOSX') - global_time = [] - watts_used = [] - current_charge = [] - draw30 = True - with open('battery_info.txt', 'r') as file: - for line in file: - data = line.strip('\n').split(' ') - global_time.append(float(data[0])) - watts_used.append(float(data[1])) - current_charge.append(float(data[2])) - if float(data[0]) > 30 and draw30: - draw_graphs(global_time, watts_used, current_charge) - draw30 = False - draw_graph(global_time, watts_used) - draw_graphs(global_time, watts_used, current_charge) + + COLUMNS = [ + "global_time", + "watts_used", + "current_charge" + ] + + mypath = "data/babyB/" + onlyfiles = [f for f in listdir(mypath) if f.endswith('.txt')] + + n_iter = 120 + robot_average = None + for file_name in onlyfiles: + global_time = [] + watts_used = [] + current_charge = [] + + draw30 = False # plot for first 30 seconds + drawIter = True # plot for each iteration (60s) + i = 1 + temp_w = [] + temp_c = [] + temp_diff = [] + + + + with open(mypath + file_name, 'r') as file: + for line in file: + data = line.strip('\n').split(' ') + global_time.append(float(data[0])) + watts_used.append(float(data[1])) + current_charge.append(float(data[2])) + + if drawIter: + temp_w.append(float(data[1])) + temp_c.append(float(data[2])) + if float(data[0]) > i * 60 : + temp_diff.append(temp_c[0]-temp_c[-1]) + # draw_graphs(file_name +" iteration: "+ str(i),range(len(temp_c)), temp_w, temp_c) + # draw_graph(file_name +" iteration: "+ str(i),range(len(temp_c)), temp_w) + i += 1 + temp_c.clear() + temp_w.clear() + + if float(data[0]) > 500 and draw30: + draw_graphs(file_name, global_time, watts_used, current_charge) + draw30 = False + # draw_graph(file_name, global_time, watts_used) + # draw_graphs(file_name, global_time, watts_used, current_charge) + + + plt.plot(range(1, 121), temp_diff,marker='x') + # plt.axis([0,120,6.4,14]) + plt.xlabel('iteration number') + plt.ylabel('total energy used (joules)') + plt.show() + + if robot_average is None: + robot_average = np.array(temp_diff) + else: + robot_average += np.array(temp_diff) + + + + robot_average /= 10 + plt.title("the average of joules used per iteration of all robots") + plt.plot(range(1, 121), robot_average ,marker='x') + # plt.axis([0,120,6.4,14]) + plt.xlabel('iteration number') + plt.ylabel('total energy used (joules)') + plt.show() if __name__ == "__main__": main() \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index d8059f348e..142b17a32b 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -283,14 +283,17 @@ 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§\n"; + this->directory_name = "output/cpg_bo/babyA/"; // CHANGETHIS + this->directory_name += this->battery_->time_init + "/"; } + else + std::cout << "§no§\n"; - std::system(("mkdir -p " + this->directory_name).c_str()); + std::system(("mkdir -p " + this->directory_name).c_str()); // Initialise array of neuron states for Update() method this->next_state = new double[this->neurons.size()]; @@ -375,8 +378,17 @@ DifferentialCPG::~DifferentialCPG() */ struct DifferentialCPG::evaluation_function{ // Number of input dimension (samples.size()) - //spider9:18,spider13:26,spider17:34,gecko7:13,gecko12:23,gecko17:33,babyA:16,babyB:22,babyC:32,one+:12 - BO_PARAM(size_t, dim_in, 32); // STEP 2 + // spider9:18, + // spider13:26, + // spider17:34, + // gecko7:13, + // gecko12:23, + // gecko17:33, + // babyA:16, + // babyB:22, + // babyC:32, + // one+:12 + BO_PARAM(size_t, dim_in, 16); // CHANGETHIS // number of dimensions of the fitness BO_PARAM(size_t, dim_out, 1); @@ -413,6 +425,9 @@ void DifferentialCPG::bo_init_sampling(){ Eigen::VectorXd init_sample(this->n_weights); // For all weights + srand((unsigned)time(NULL)); + // trash first one, because it could be the same and I do not know why + auto trash_first = rand(); for (size_t j = 0; j < this->n_weights; j++) { // Generate a random number in [0, 1]. Transform later @@ -460,6 +475,9 @@ void DifferentialCPG::bo_init_sampling(){ Eigen::VectorXd init_sample(this->n_weights); // For all dimensions + srand((unsigned)time(NULL)); + // trash first one, because it could be the same and I do not know why + auto trash_first = rand(); for (size_t j = 0; j < this->n_weights; j++) { // Take a LHS diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp index cce5deb450..cca46aa6f3 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp @@ -35,7 +35,6 @@ double Evaluator::measure_distance( return std::sqrt(std::pow(_pose1.Pos().X() - _pose2.Pos().X(), 2) + std::pow(_pose1.Pos().Y() - _pose2.Pos().Y(), 2)); } - ///////////////////////////////////////////////// Evaluator::Evaluator(std::shared_ptr<::revolve::gazebo::Battery> battery, const double _evaluationRate, @@ -72,6 +71,7 @@ void Evaluator::Reset() double Evaluator::Fitness() { double fitness_value = 0.0; + double speed; if(this->locomotion_type == "gait") { double dS; @@ -100,8 +100,13 @@ double Evaluator::Fitness() } coordinates << std::fixed << pose_i.Pos().X() << " " << pose_i.Pos().Y() << std::endl; } + // save the speed to speed.txt + std::ofstream speeds; + speeds.open(this->directory_name + "/speed.txt",std::ios::app); + speed = this->path_length / evaluation_rate_; + speeds << speed << std::endl; - ////********** directed locomotion fitness function **********//// + ////********** directed locomotion fitness function **********//// //directions(forward) of heads are the orientation(+x axis) - 1.570796 double beta0 = this->start_position_.Rot().Yaw()- M_PI/2.0; if (beta0 < - M_PI) //always less than pi (beta0 + max(40degree) < pi) diff --git a/experiments/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index e5dd9789cd..d36357729d 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -24,7 +24,7 @@ n_runs = 10 # Naar 20 n_jobs = 4 my_yaml_path = "experiments/bo_learner/yaml/" -yaml_model = "babyC.yaml" +yaml_model = "babyA.yaml" # CHANGETHIS manager = "experiments/bo_learner/manager.py" python_interpreter = "~/projects/revolve/.venv/bin/python3" search_space = { @@ -46,7 +46,7 @@ # You don't have to change this my_sub_directory = "yaml_temp/" output_path = "output/cpg_bo/main_" + str(round(time.time())) + "/" -start_port = 11000 # STEP 6 +start_port = 13000 # STEP 6 finished = False # Make in revolve/build to allow runs from terminal From 35e9d50a0225758db083db5f654088ee8e43f59c Mon Sep 17 00:00:00 2001 From: roy Date: Mon, 22 Jul 2019 17:42:58 +0200 Subject: [PATCH 07/19] fixed directory for battery data and speed data --- cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt | 0 cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt | 0 cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt | 0 cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt | 0 cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt | 0 cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt | 0 cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt | 0 cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt | 0 cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt | 0 cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp | 2 +- 10 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt create mode 100644 cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt diff --git a/cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 142b17a32b..21ac7e49c8 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -295,7 +295,7 @@ DifferentialCPG::DifferentialCPG( std::system(("mkdir -p " + this->directory_name).c_str()); - // Initialise array of neuron states for Update() method + // Initialise array of neuron states for Update() methodc this->next_state = new double[this->neurons.size()]; this->n_weights = (int)(this->connections.size()/2) + this->n_motors; From dd5daa2c1a2d2ce97c0e49defe05894669454179 Mon Sep 17 00:00:00 2001 From: roy Date: Mon, 22 Jul 2019 17:55:38 +0200 Subject: [PATCH 08/19] fixed directory for battery data and speed data --- experiments/bo_learner/GridSearch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/experiments/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index d36357729d..098d88ad89 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -24,7 +24,7 @@ n_runs = 10 # Naar 20 n_jobs = 4 my_yaml_path = "experiments/bo_learner/yaml/" -yaml_model = "babyA.yaml" # CHANGETHIS +yaml_model = "babyA.yaml" # CHANGETHIS ! manager = "experiments/bo_learner/manager.py" python_interpreter = "~/projects/revolve/.venv/bin/python3" search_space = { From 81484c8f855ef8671982bf52293193e308661537 Mon Sep 17 00:00:00 2001 From: roy-basmacier Date: Thu, 25 Jul 2019 13:14:30 +0200 Subject: [PATCH 09/19] fixed directory for battery data and speed data --- cpprevolve/revolve/gazebo/battery/Battery.cpp | 8 +- cpprevolve/revolve/gazebo/battery/Battery.h | 6 +- .../revolve/gazebo/battery/battery_chart.py | 59 ------- cpprevolve/revolve/gazebo/battery/charts.py | 97 ++++++++++++ .../revolve/gazebo/battery/temp_charts.py | 148 ++++++++++++++++++ .../revolve/gazebo/brains/DifferentialCPG.cpp | 35 ++++- .../revolve/gazebo/brains/Evaluator.cpp | 134 ++++++++++++++-- .../revolve/gazebo/motors/PositionMotor.cpp | 2 +- .../revolve/gazebo/plugin/RobotController.cpp | 3 +- experiments/bo_learner/GridSearch.py | 16 +- experiments/bo_learner/yaml/babyA.yaml | 6 +- experiments/bo_learner/yaml/babyB.yaml | 6 +- experiments/bo_learner/yaml/babyC.yaml | 6 +- experiments/bo_learner/yaml/gecko12.yaml | 6 +- experiments/bo_learner/yaml/gecko17.yaml | 6 +- experiments/bo_learner/yaml/gecko7.yaml | 6 +- experiments/bo_learner/yaml/spider13.yaml | 6 +- experiments/bo_learner/yaml/spider17.yaml | 6 +- experiments/bo_learner/yaml/spider9.yaml | 6 +- 19 files changed, 442 insertions(+), 120 deletions(-) create mode 100644 cpprevolve/revolve/gazebo/battery/charts.py create mode 100644 cpprevolve/revolve/gazebo/battery/temp_charts.py diff --git a/cpprevolve/revolve/gazebo/battery/Battery.cpp b/cpprevolve/revolve/gazebo/battery/Battery.cpp index 0f947ba47f..58999d72cb 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.cpp +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -7,7 +7,7 @@ using namespace revolve::gazebo; Battery::Battery(double initial_charge) - : initial_charge(initial_charge), current_charge(initial_charge), watts_used(0), time_init(std::to_string(time(0))) + : 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) @@ -20,9 +20,9 @@ void Battery::Update(double global_time, double delta_time) } this->current_charge += sum * delta_time; // charge is measured in joules std::ofstream b_info_file; - // CHANGETHIS - b_info_file.open("cpprevolve/revolve/gazebo/battery/data/babyA/battery_info_" + this->time_init + ".txt", std::ios_base::app); - + 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() << std::endl; b_info_file << global_time << " " << sum << " " << current_charge << std::endl; // std::cout << this->time_init<< std::endl; } diff --git a/cpprevolve/revolve/gazebo/battery/Battery.h b/cpprevolve/revolve/gazebo/battery/Battery.h index 017b4dfc99..94ff253fbd 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.h +++ b/cpprevolve/revolve/gazebo/battery/Battery.h @@ -30,12 +30,12 @@ class Battery : public ::gazebo::common::Battery /// \brief current charge of the battery in joules double current_charge; - /// \brief amount of watts used for all servos at a time - double watts_used; - /// \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; }; diff --git a/cpprevolve/revolve/gazebo/battery/battery_chart.py b/cpprevolve/revolve/gazebo/battery/battery_chart.py index d053525ac9..7928912187 100644 --- a/cpprevolve/revolve/gazebo/battery/battery_chart.py +++ b/cpprevolve/revolve/gazebo/battery/battery_chart.py @@ -9,65 +9,6 @@ from scipy.ndimage.filters import gaussian_filter1d -def nice_chart(col_name, do_log=False, do_exp=False, axs=None, col_normalize=None): - n = 3 - viridis = matplotlib.cm.get_cmap('viridis', n) - colormap = viridis(np.linspace(0, 1, n)) - if axs is None: - fig, axs = plt.subplots(1,1, figsize=(8,8)) - - title = col_name - if col_normalize is not None: - title = col_name + ' / ' + col_normalize - if do_log: - title = title + " (log)" - elif do_exp: - title = title + " (exp)" - - - axs.set_title(title) - - for_list = [ - (0, 'direct', direct, axs, colormap[0]), - (1, 'lsystem', lsystem, axs, colormap[1]), - (2, 'cppn', cppn, axs, colormap[2]), - ] - - for i, title, data, ax, color in for_list: - values = [] - for name,group in data.groupby('gen'): - line_values = group[col_name] - if col_normalize is not None: - line_values = line_values / group[col_normalize] - ax.set_ylim((0,1)) - if do_log: - line_values = np.log(line_values) - elif do_exp: - line_values = np.exp(line_values) - - values.append([np.average(line_values)] + np.percentile(line_values, [0, 25, 50, 75, 100]).tolist()) - values = pd.DataFrame(data=values, columns=['avg','0','25','50','75','100']) - - x = np.arange(600) - #ax.set_title(title) - ax.plot(values['avg'], linewidth=2, color=color, label=title) - ax.fill_between(x, values['25'], values['75'], alpha=0.4, color=color) - - offset= i*10 - x = np.arange(offset, 599, 60) - yerr_top = [] - yerr_bottom = [] - yerr_interval = [] - for i in range(len(x)): - yerr_top.append(values['avg'][x[i]] - values['100'][x[i]]) - yerr_bottom.append(values['0'][x[i]] - values['avg'][x[i]]) - yerr_interval.append(values['avg'][x[i]]) - ax.errorbar(x, yerr_interval, yerr=[yerr_top, yerr_bottom], alpha=0.4, capsize=2, color=color); - - axs.legend(fontsize=12, loc="lower right", bbox_to_anchor=[1, 0], - ncol=2, shadow=True, fancybox=True) - - def average(n, lst): res = [] for i in range(len(lst)//n): diff --git a/cpprevolve/revolve/gazebo/battery/charts.py b/cpprevolve/revolve/gazebo/battery/charts.py new file mode 100644 index 0000000000..9c9199a06f --- /dev/null +++ b/cpprevolve/revolve/gazebo/battery/charts.py @@ -0,0 +1,97 @@ +import matplotlib +import matplotlib.pyplot as plt +import numpy as np +from os import listdir +from scipy.optimize import curve_fit +import pandas as pd + +from scipy.ndimage.filters import gaussian_filter1d + + +def draw_graph(title, xlabel, x, ylabel, y, avg = False): + plt.title(title) + plt.plot(x, y) + plt.ylabel(ylabel) + plt.xlabel(xlabel) + if avg: + plt.plot(x, gaussian_filter1d(y, sigma=3)) + plt.show() + + +def draw_both_gaussian(y1, y2, title = ''): + + plt.subplot(2, 1, 1) + if title == '': + plt.title('gaussian filter of average speed and energy used per iterations') + else: + plt.title(title) + plt.ylabel('energy') + plt.plot(range(1,121), y1, label='energy') + plt.plot(range(1,121), gaussian_filter1d(y1, sigma=3), label='energy') + + plt.subplot(2, 1, 2) + plt.ylabel('speed') + plt.xlabel('iteration') + plt.plot(range(1,121), y2, label='energy') + plt.plot(range(1,121), gaussian_filter1d(y2, sigma=3), label='speed') + plt.show() + + +def main(): + # matplotlib.use('MacOSX') + robots = listdir('data') + robots.remove('.DS_Store') + + battery_path = 'data/' + output_path = '../../../../output/cpg_bo/' + + for robot in robots: + + + + battery_info = [f for f in listdir(battery_path + robot) if f.endswith('.txt')] + + for file_name in battery_info: + global_time = [] + watts_used = [] + current_charge = [] + temp_w = [] + temp_c = [] + temp_diff = [] + i = 1 + with open(battery_path + robot + '/' + file_name, 'r') as file: + for line in file: + data = line.strip('\n').split(' ') + global_time.append(float(data[0])) + watts_used.append(float(data[1])) + current_charge.append(float(data[2])) + + temp_w.append(float(data[1])) + temp_c.append(float(data[2])) + + if float(data[0]) > i * 60: + temp_diff.append(temp_c[0]-temp_c[-1]) + i += 1 + temp_c.clear() + temp_w.clear() + + df = pd.DataFrame({'global time':global_time, 'watts used':watts_used, 'current charge':current_charge}) + print(df.quantile([.1, .25, .5, .75], axis = 1)) + quit(0) + + + num_info = listdir(output_path + robot) + num_info.remove('.DS_Store') + for num in num_info: + with open(output_path + robot + '/' + num + '/speed.txt', 'r') as file: + speed = [] + for line in file: + data = line.strip('\n') + speed.append(float(data)) + + + + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/battery/temp_charts.py b/cpprevolve/revolve/gazebo/battery/temp_charts.py new file mode 100644 index 0000000000..84707d1e9f --- /dev/null +++ b/cpprevolve/revolve/gazebo/battery/temp_charts.py @@ -0,0 +1,148 @@ +import matplotlib +import matplotlib.pyplot as plt +import numpy as np +from os import listdir +from scipy.optimize import curve_fit +import pandas as pd + +from scipy.ndimage.filters import gaussian_filter1d + + +def draw_graph(title, xlabel, x, ylabel, y, avg = False): + plt.title(title) + plt.plot(x, y) + plt.ylabel(ylabel) + plt.xlabel(xlabel) + if avg: + plt.plot(x, gaussian_filter1d(y, sigma=3)) + plt.show() + + +def draw_both_gaussian(y1, y2, title = ''): + + plt.subplot(2, 1, 1) + if title == '': + plt.title('gaussian filter of average speed and energy used per iterations') + else: + plt.title(title) + plt.ylabel('energy') + plt.plot(range(1,121), y1, label='energy') + plt.plot(range(1,121), gaussian_filter1d(y1, sigma=3), label='energy') + + plt.subplot(2, 1, 2) + plt.ylabel('speed') + plt.xlabel('iteration') + plt.plot(range(1,121), y2, label='energy') + plt.plot(range(1,121), gaussian_filter1d(y2, sigma=3), label='speed') + plt.show() + + +def main(): + # matplotlib.use('MacOSX') + robots = listdir('data') + robots.remove('.DS_Store') + + battery_path = 'data/' + output_path = '../../../../output/cpg_bo/' + + all_robot_battery_avg = None + all_robot_speed_avg = None + + for robot in robots: + + robot_battery_avg = None + robot_speed_avg = None + + battery_info = [f for f in listdir(battery_path + robot) if f.endswith('.txt')] + + for file_name in battery_info: + global_time = [] + watts_used = [] + current_charge = [] + temp_w = [] + temp_c = [] + temp_diff = [] + i = 1 + with open(battery_path + robot + '/' + file_name, 'r') as file: + for line in file: + data = line.strip('\n').split(' ') + global_time.append(float(data[0])) + watts_used.append(float(data[1])) + current_charge.append(float(data[2])) + + temp_w.append(float(data[1])) + temp_c.append(float(data[2])) + + if float(data[0]) > i * 60: + temp_diff.append(temp_c[0]-temp_c[-1]) + i += 1 + temp_c.clear() + temp_w.clear() + + # draw_graph('energy used per iteration for ' + robot + '\n iteration: ' + file_name, + # 'iteration', range(1, 121), 'total energy used', temp_diff) + + if robot_battery_avg is None: + robot_battery_avg = np.array(temp_diff) + else: + robot_battery_avg += np.array(temp_diff) + + robot_battery_avg /= 10 + # draw_graph('energy used per iteration for ' + robot + '\n average of all iterations ', + # 'iteration', range(1, 121), 'total energy used', robot_battery_avg) + + + + num_info = listdir(output_path + robot) + num_info.remove('.DS_Store') + for num in num_info: + with open(output_path + robot + '/' + num + '/speed.txt', 'r') as file: + speed = [] + for line in file: + data = line.strip('\n') + speed.append(float(data)) + + # draw_graph('speed for ' + robot + '\n iteration: ' + num, 'iteration', range(1,121), 'speed', speed) + + if robot_speed_avg is None: + robot_speed_avg = np.array(speed) + else: + robot_speed_avg += np.array(speed) + + robot_speed_avg /= 10 + # draw_graph('speed for ' + robot + '\n average of all iterations ', 'iteration', range(1,121), 'speed', robot_speed_avg) + + # draw_both_gaussian(robot_battery_avg, robot_speed_avg, 'Robot: ' + robot) + + if all_robot_battery_avg is None: + all_robot_battery_avg = np.array(robot_battery_avg) + else: + all_robot_battery_avg += np.array(robot_battery_avg) + + if all_robot_speed_avg is None: + all_robot_speed_avg = np.array(robot_speed_avg) + else: + all_robot_speed_avg += np.array(robot_speed_avg) + + all_robot_speed_avg /= 10 + all_robot_battery_avg /= 10 + + draw_graph('average speed for all robots per iteration', 'iterations', range(1, 121), 'average speed', all_robot_speed_avg, True) + + draw_graph('average energy used for all robots per iteration', 'iteration', range(1, 121), 'average energy used', all_robot_battery_avg, True) + + draw_both_gaussian(all_robot_battery_avg, all_robot_speed_avg) + + ratio = [] + for i in range(len(all_robot_speed_avg)): + ratio.append(all_robot_speed_avg[i]/all_robot_battery_avg[i]) + + draw_graph('ratio of average of all speed / average of all energy', 'iteration', range(1, 121), 'speed/energy', ratio, True) + + + + + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 21ac7e49c8..4c60ad5c6b 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -64,6 +64,9 @@ using Init_t = limbo::init::LHS; using Kernel_t = limbo::kernel::MaternFiveHalves; using GP_t = limbo::model::GP; + + + /** * Constructor for DifferentialCPG class. * @@ -282,17 +285,21 @@ DifferentialCPG::DifferentialCPG( } } + + // Create directory for output. // this->directory_name = controller->GetAttribute("output_directory")->GetAsString(); if(this->directory_name.empty()) { - std::cout << "§yes§\n"; - this->directory_name = "output/cpg_bo/babyA/"; // CHANGETHIS + 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()); // Initialise array of neuron states for Update() methodc @@ -376,6 +383,8 @@ DifferentialCPG::~DifferentialCPG() /** * Dummy function for limbo */ + + struct DifferentialCPG::evaluation_function{ // Number of input dimension (samples.size()) // spider9:18, @@ -388,7 +397,7 @@ struct DifferentialCPG::evaluation_function{ // babyB:22, // babyC:32, // one+:12 - BO_PARAM(size_t, dim_in, 16); // CHANGETHIS + BO_PARAM(size_t, dim_in, 0); // CHANGETHIS // number of dimensions of the fitness BO_PARAM(size_t, dim_out, 1); @@ -658,6 +667,26 @@ void DifferentialCPG::bo_step(){ limbo::acquifun>> boptimizer; // Optimize. Pass dummy evaluation function and observations . + + if (this->robot->GetName() == "example_spider") + const int DIM_IN = 18; + else if (this->robot->GetName() == "example_spider13") + const int DIM_IN = 26; + else if (this->robot->GetName() == "example_spider17") + const int DIM_IN = 34; + else if (this->robot->GetName() == "example_gecko7") + const int DIM_IN = 13; + else if (this->robot->GetName() == "example_gecko12") + const int DIM_IN = 23; + else if (this->robot->GetName() == "example_gecko17") + const int DIM_IN = 33; + else if (this->robot->GetName() == "example_babyA") + const int DIM_IN = 16; + else if (this->robot->GetName() == "example_babyB") + const int DIM_IN = 22; + else if (this->robot->GetName() == "example_babyC") + const int DIM_IN = 32; + boptimizer.optimize(DifferentialCPG::evaluation_function(), this->samples, this->observations); diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp index cca46aa6f3..71fcd68b89 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp @@ -51,7 +51,7 @@ Evaluator::Evaluator(std::shared_ptr<::revolve::gazebo::Battery> battery, this->current_position_.Reset(); this->previous_position_.Reset(); this->start_position_.Reset(); - this->locomotion_type = "directed"; // {directed, gait, battery} // STEP 3 + this->locomotion_type = "battery"; // {directed, gait, battery} // STEP 3 this->path_length = 0.0; } @@ -174,27 +174,121 @@ double Evaluator::Fitness() } else if (this->locomotion_type == "battery") { - // f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t - (len * size(13-34) * P) ) ] + this->step_poses.push_back(this->current_position_); + //step_poses: x y z roll pitch yaw + for (int i=1; i < this->step_poses.size(); i++) + { + const auto &pose_i_1 = this->step_poses[i-1]; + const auto &pose_i = this->step_poses[i]; + this->path_length += Evaluator::measure_distance(pose_i_1, pose_i); + //save coordinations to coordinates.txt + std::ofstream coordinates; + coordinates.open(this->directory_name + "/coordinates.txt",std::ios::app); + + if(i == 1) + { + coordinates << std::fixed << start_position_.Pos().X() << " " << start_position_.Pos().Y() << std::endl; + } + coordinates << std::fixed << pose_i.Pos().X() << " " << pose_i.Pos().Y() << std::endl; + } + // save the speed to speed.txt + std::ofstream speeds; + speeds.open(this->directory_name + "/speed.txt",std::ios::app); + speed = this->path_length / evaluation_rate_; + speeds << speed << std::endl; + + ////********** directed locomotion fitness function **********//// + //directions(forward) of heads are the orientation(+x axis) - 1.570796 + double beta0 = this->start_position_.Rot().Yaw()- M_PI/2.0; + if (beta0 < - M_PI) //always less than pi (beta0 + max(40degree) < pi) + { + beta0 = 2 * M_PI - std::abs(beta0); + } + + //save direction to coordinates.txt: This is used to make Figure 8 + std::ofstream coordinates; + coordinates.open(this->directory_name + "/coordinates.txt",std::ios::app); + coordinates << std::fixed << beta0 << std::endl; + + double beta1 = std::atan2( + this->current_position_.Pos().Y() - this->start_position_.Pos().Y(), + this->current_position_.Pos().X() - this->start_position_.Pos().X()); + + double alpha; + if (std::abs(beta1 - beta0) > M_PI) + { + alpha = 2 * M_PI - std::abs(beta1) - std::abs(beta0); + } else + { + alpha = std::abs(beta1 - beta0); + } + + double A = std::tan(beta0); + double B = this->start_position_.Pos().Y() + - A * this->start_position_.Pos().X(); + + double X_p = (A * (this->current_position_.Pos().Y() - B) + + this->current_position_.Pos().X()) / (A * A + 1); + double Y_p = A * X_p + B; + + //calculate the fitness_direction based on dist_projection, alpha, penalty + double dist_projection; + double dist_penalty; + double penalty; + double fitness_direction; + double ksi = 1.0; + if (alpha > 0.5 * M_PI) + { + dist_projection = -std::sqrt( + std::pow((this->start_position_.Pos().X() - X_p), 2.0) + + std::pow((this->start_position_.Pos().Y() - Y_p), 2.0)); + dist_penalty = std::sqrt( + std::pow((this->current_position_.Pos().X() - X_p), 2.0) + + std::pow((this->current_position_.Pos().Y() - Y_p), 2.0)); + penalty = 0.01 * dist_penalty; + } + else + { + dist_projection = std::sqrt( + std::pow((this->start_position_.Pos().X() - X_p), 2.0) + + std::pow((this->start_position_.Pos().Y() - Y_p), 2.0)); + dist_penalty = std::sqrt( + std::pow((this->current_position_.Pos().X() - X_p), 2.0) + + std::pow((this->current_position_.Pos().Y() - Y_p), 2.0)); + penalty = 0.01 * dist_penalty; + } + + //fitness_direction = dist_projection / (alpha + ksi) - penalty; + fitness_direction = std::abs(dist_projection) / path_length * + (dist_projection / (alpha + ksi) - penalty); + + double w = 0.03; + std::cout << "f:\t" << fitness_direction << "\tP:\t" << (this->battery_->current_charge * w) << std::endl; + fitness_value = fitness_direction + (this->battery_->current_charge * w); + this->battery_->current_charge = 0; /// reseting the charge of the robot + } + else if (this->locomotion_type == "battery_targeted") + { + // old_f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t - (len * size(13-34) * P) ) ] + + // new_f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t + P) ) ] double initial_battery = 1; // b -> initial battery level. 1 (100%) double battery_time = 0.15; // b_t -> battery drainage, how much battery robot loses when idle (over time) double distance_projection; // D_p -> distance projection, the path the robot takes double shortest_distance; // D -> the shortest path to target // len -> this->path_length double robot_size = 17; // size -> the size of the robot TODO get the size from the robot manager - double power_used = this->battery_->watts_used; // P -> the amount of power used by each servo motor + double power_used = this->battery_->current_charge; // P -> the amount of power used by each servo motor // initializing the coordinates of the charging station (±1.6, ±1.6, 0.12) (x,y,z) ignition::math::Pose3d target_coord; target_coord.Pos().X() = 1.6; target_coord.Pos().Y() = 1.6; target_coord.Pos().Z() = 0.12; + // calculating the shortest/initial distance shortest_distance = measure_distance(this->start_position_, target_coord); -// std::cout << "b: " << initial_battery << "\n"; -// std::cout << "b_t: " << battery_time << "\n"; -// std::cout << "D: " << shortest_distance << "\n"; -// std::cout << "size: " << robot_size << "\n"; - std::cout << "P: " << power_used << "\n"; + @@ -221,8 +315,7 @@ double Evaluator::Fitness() coordinates << std::fixed << pose_i.Pos().X() << " " << pose_i.Pos().Y() << std::endl; } -// std::cout << "len: " << this->path_length << "\n"; -// + // calculating the distance projection ////********** directed locomotion fitness function **********//// @@ -269,16 +362,27 @@ double Evaluator::Fitness() std::pow((this->start_position_.Pos().Y() - Y_p), 2.0)); } // ************************ - std::cout << "D_p: " << distance_projection << "\t"; - std::cout << "D: " << shortest_distance << " ratio:" << distance_projection/shortest_distance << "\n"; - //f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t - (len * size(13-34) * P) ) ] + //f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t + (len * size(13-34) * P) ) ] fitness_value = (distance_projection/shortest_distance) * initial_battery - - ((1 - (distance_projection/shortest_distance) ) * (battery_time + (this->path_length * robot_size * power_used))); + ((1 - (distance_projection/shortest_distance) ) * (battery_time + (this->path_length * robot_size * -power_used))); + +// std::cout << "b: " << initial_battery << "\n"; +// std::cout << "size: " << robot_size << "\n"; +// std::cout << "b_t: " << battery_time << "\n"; + + std::cout << "D_p: " << distance_projection << "\t"; +// std::cout << "len: " << this->path_length << "\n"; +// std::cout << "D: " << shortest_distance << "\n"; +// std::cout << "start pos: " << this->start_position_ << std::endl; + + std::cout << "P: " << power_used << "\n"; + + std::cout << "distance_projection/shortest_distance = " << distance_projection/shortest_distance << std::endl; std::cout << "fitness: " << fitness_value << "\n"; - this->battery_->watts_used = 0.0; + this->battery_->current_charge = 0; /// changing its charge to 0 to simulate the reseting of the robot } return fitness_value; } diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index 1f315a6502..58b9e19cd0 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -138,7 +138,7 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) // std::cout << "dot (local): " << this->joint_->LocalAxis(0).Dot(jointWrench.body1Torque) * cmd << '\t'; // dot product of torque and local axis // std::cout << "dot (global): " << this->joint_->GlobalAxis(0).Dot(jointWrench.body1Torque) * cmd << std::endl; - // TODO find wich axis to use local or global + // TODO find which axis to use local or global // TODO check the watt if it should be positive or negative // TODO change this for now im using the absolute value of the watt so it always decreases from the joint movements double watt = -abs(cmd * jointWrench.body1Torque.Length()); // TODO check which torque to use 1 or 2 diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 3e1828778e..eef9e16a4a 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -229,7 +229,6 @@ void RobotController::DoUpdate(const ::gazebo::common::UpdateInfo _info) // { // std::exit(0); // } - this->brain_->Update(motors_, sensors_, currentTime, actuationTime_); this->battery_->Update(currentTime, actuationTime_); } @@ -252,6 +251,8 @@ void RobotController::LoadBattery(const sdf::ElementPtr _sdf) this->battery_.reset(new ::revolve::gazebo::Battery(battery_initial_charge)); // set initial battery (joules) this->battery_->UpdateParameters(batteryElem); this->battery_->ResetVoltage(); + this->battery_->robot_name = this->model_->GetName(); + // this->battery_->SetUpdateFunc([](const ::gazebo::common::BatteryPtr &battery) -> double { // }); } diff --git a/experiments/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index 098d88ad89..bd7b441dad 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -19,12 +19,12 @@ # Parameters -min_lines = 110 +min_lines = 1490 run_gazebo = False -n_runs = 10 # Naar 20 +n_runs = 15 # Naar 20 n_jobs = 4 my_yaml_path = "experiments/bo_learner/yaml/" -yaml_model = "babyA.yaml" # CHANGETHIS ! +yaml_model = "spider9.yaml" # CHANGETHIS ! manager = "experiments/bo_learner/manager.py" python_interpreter = "~/projects/revolve/.venv/bin/python3" search_space = { @@ -34,8 +34,8 @@ 'verbose': [1], 'kernel_l': [0.1], 'acqui_ucb_alpha': [1.0], - 'n_learning_iterations': [100], - 'n_init_samples': [20], + 'n_learning_iterations': [1450], + 'n_init_samples': [50], # 'kernel_l': [0.2], # 'acqui_ucb_alpha': [3.0], # 'n_learning_iterations': [950], @@ -134,14 +134,16 @@ def run(i, sub_directory, model, params): " ./revolve.py" + \ " --manager " + manager + \ " --world-address " + world_address + \ - " --robot-yaml " + yaml_model + " --robot-yaml " + yaml_model + \ + " --port-start " + str(start_port + i) else: py_command = python_interpreter + \ " ./revolve.py" + \ " --manager " + manager + \ " --world-address " + world_address + \ " --robot-yaml " + yaml_model + \ - " --simulator-cmd gazebo" + " --simulator-cmd gazebo" + \ + " --port-start " + str(start_port + i) return_code = os.system(py_command) if return_code == 32512: diff --git a/experiments/bo_learner/yaml/babyA.yaml b/experiments/bo_learner/yaml/babyA.yaml index 7b29507ff8..9c2960b16e 100644 --- a/experiments/bo_learner/yaml/babyA.yaml +++ b/experiments/bo_learner/yaml/babyA.yaml @@ -163,11 +163,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -184,7 +184,7 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/babyB.yaml b/experiments/bo_learner/yaml/babyB.yaml index 90883533b5..99edca2d34 100644 --- a/experiments/bo_learner/yaml/babyB.yaml +++ b/experiments/bo_learner/yaml/babyB.yaml @@ -195,11 +195,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -213,7 +213,7 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/babyC.yaml b/experiments/bo_learner/yaml/babyC.yaml index 55ff08464f..a8b56ebe9a 100644 --- a/experiments/bo_learner/yaml/babyC.yaml +++ b/experiments/bo_learner/yaml/babyC.yaml @@ -309,11 +309,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -330,7 +330,7 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/gecko12.yaml b/experiments/bo_learner/yaml/gecko12.yaml index 7c8ab72a74..9a39d5bb3c 100644 --- a/experiments/bo_learner/yaml/gecko12.yaml +++ b/experiments/bo_learner/yaml/gecko12.yaml @@ -226,11 +226,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -244,7 +244,7 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/gecko17.yaml b/experiments/bo_learner/yaml/gecko17.yaml index 263564ccae..926ee2d56c 100644 --- a/experiments/bo_learner/yaml/gecko17.yaml +++ b/experiments/bo_learner/yaml/gecko17.yaml @@ -318,11 +318,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -336,7 +336,7 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/gecko7.yaml b/experiments/bo_learner/yaml/gecko7.yaml index e58dfe0bcc..0546e82685 100644 --- a/experiments/bo_learner/yaml/gecko7.yaml +++ b/experiments/bo_learner/yaml/gecko7.yaml @@ -135,11 +135,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -153,7 +153,7 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/spider13.yaml b/experiments/bo_learner/yaml/spider13.yaml index 2244f9f1f7..631c67dbf7 100644 --- a/experiments/bo_learner/yaml/spider13.yaml +++ b/experiments/bo_learner/yaml/spider13.yaml @@ -231,11 +231,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -249,7 +249,7 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/spider17.yaml b/experiments/bo_learner/yaml/spider17.yaml index 2c292422ed..ee8fe6fa4c 100644 --- a/experiments/bo_learner/yaml/spider17.yaml +++ b/experiments/bo_learner/yaml/spider17.yaml @@ -303,11 +303,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -321,7 +321,7 @@ brain: meta: robot_size: 34 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 diff --git a/experiments/bo_learner/yaml/spider9.yaml b/experiments/bo_learner/yaml/spider9.yaml index 52629eb41f..b1c729ee26 100644 --- a/experiments/bo_learner/yaml/spider9.yaml +++ b/experiments/bo_learner/yaml/spider9.yaml @@ -155,11 +155,11 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.0 + range_lb: -1.5 range_ub: 1.5 init_neuron_state: 0.707 learner: - n_init_samples: 20 + n_init_samples: 50 init_method: "LHS" kernel_noise: 0.00000001 kernel_optimize_noise: "false" @@ -176,7 +176,7 @@ brain: meta: robot_size: 18 run_analytics: "true" - n_learning_iterations: 100 + n_learning_iterations: 1450 n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 From f2f0dd898ad4772aabced521b84195f6d6b827e3 Mon Sep 17 00:00:00 2001 From: roy-basmacier Date: Mon, 12 Aug 2019 02:05:31 +0200 Subject: [PATCH 10/19] fixed merge requests --- cpprevolve/Makefile | 184 --- cpprevolve/Revolve.cbp | 997 ------------- cpprevolve/cmake_install.cmake | 41 - cpprevolve/revolve/gazebo/Makefile | 1238 ----------------- cpprevolve/revolve/gazebo/battery/Battery.cpp | 5 +- .../revolve/gazebo/battery/battery_chart.py | 123 -- .../gazebo/battery/data/babyA/delete_this.txt | 0 .../gazebo/battery/data/babyB/delete_this.txt | 0 .../gazebo/battery/data/babyC/delete_this.txt | 0 .../battery/data/gecko12/delete_this.txt | 0 .../battery/data/gecko17/delete_this.txt | 0 .../battery/data/gecko7/delete_this.txt | 0 .../battery/data/spider13/delete_this.txt | 0 .../battery/data/spider17/delete_this.txt | 0 .../battery/data/spider9/delete_this.txt | 0 .../revolve/gazebo/brains/DifferentialCPG.cpp | 23 +- .../revolve/gazebo/brains/Evaluator.cpp | 9 +- .../revolve/gazebo/motors/PositionMotor.cpp | 2 - .../revolve/gazebo/plugin/RobotController.cpp | 2 - experiments/bo_learner/GridSearch.py | 88 +- experiments/bo_learner/yaml/babyA.yaml | 4 +- experiments/roys_experiments/GridSearch.py | 348 +++++ .../roys_experiments/battery_chart.py | 0 experiments/roys_experiments/charts.py | 148 ++ experiments/roys_experiments/manager.py | 89 -- .../roys_experiments/temp_charts.py | 59 +- 26 files changed, 610 insertions(+), 2750 deletions(-) delete mode 100644 cpprevolve/Makefile delete mode 100644 cpprevolve/Revolve.cbp delete mode 100644 cpprevolve/cmake_install.cmake delete mode 100644 cpprevolve/revolve/gazebo/Makefile delete mode 100644 cpprevolve/revolve/gazebo/battery/battery_chart.py delete mode 100644 cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt delete mode 100644 cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt create mode 100644 experiments/roys_experiments/GridSearch.py rename cpprevolve/revolve/gazebo/battery/temp_charts.py => experiments/roys_experiments/battery_chart.py (100%) create mode 100644 experiments/roys_experiments/charts.py delete mode 100755 experiments/roys_experiments/manager.py rename cpprevolve/revolve/gazebo/battery/charts.py => experiments/roys_experiments/temp_charts.py (52%) diff --git a/cpprevolve/Makefile b/cpprevolve/Makefile deleted file mode 100644 index 477e05ee68..0000000000 --- a/cpprevolve/Makefile +++ /dev/null @@ -1,184 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 3.14 - -# Default target executed when no arguments are given to make. -default_target: all - -.PHONY : default_target - -# Allow only one "make -f Makefile2" at a time, but pass parallelism. -.NOTPARALLEL: - - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - - -# A target that is always out of date. -cmake_force: - -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake - -# The command to remove a file. -RM = /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /Users/roy/projects/revolve - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /Users/roy/projects/revolve - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target install/local -install/local: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake -.PHONY : install/local - -# Special rule for the target install/local -install/local/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake -.PHONY : install/local/fast - -# Special rule for the target install/strip -install/strip: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake -.PHONY : install/strip - -# Special rule for the target install/strip -install/strip/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake -.PHONY : install/strip/fast - -# Special rule for the target install -install: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -P cmake_install.cmake -.PHONY : install - -# Special rule for the target install -install/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -P cmake_install.cmake -.PHONY : install/fast - -# Special rule for the target list_install_components -list_install_components: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" -.PHONY : list_install_components - -# Special rule for the target list_install_components -list_install_components/fast: list_install_components - -.PHONY : list_install_components/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache - -.PHONY : rebuild_cache/fast - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache - -.PHONY : edit_cache/fast - -# The main all target -all: cmake_check_build_system - cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -E cmake_progress_start /Users/roy/projects/revolve/CMakeFiles /Users/roy/projects/revolve/cpprevolve/CMakeFiles/progress.marks - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/all - $(CMAKE_COMMAND) -E cmake_progress_start /Users/roy/projects/revolve/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/clean -.PHONY : clean - -# The main clean target -clean/fast: clean - -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... install/local" - @echo "... install/strip" - @echo "... install" - @echo "... list_install_components" - @echo "... rebuild_cache" - @echo "... edit_cache" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/cpprevolve/Revolve.cbp b/cpprevolve/Revolve.cbp deleted file mode 100644 index 950039aa92..0000000000 --- a/cpprevolve/Revolve.cbp +++ /dev/null @@ -1,997 +0,0 @@ - - - - - - diff --git a/cpprevolve/cmake_install.cmake b/cpprevolve/cmake_install.cmake deleted file mode 100644 index e0488848e9..0000000000 --- a/cpprevolve/cmake_install.cmake +++ /dev/null @@ -1,41 +0,0 @@ -# Install script for directory: /Users/roy/projects/revolve/cpprevolve - -# Set the install prefix -if(NOT DEFINED CMAKE_INSTALL_PREFIX) - set(CMAKE_INSTALL_PREFIX "/usr/local") -endif() -string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") - -# Set the install configuration name. -if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - if(BUILD_TYPE) - string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" - CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") - else() - set(CMAKE_INSTALL_CONFIG_NAME "Debug") - endif() - message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") -endif() - -# Set the component getting installed. -if(NOT CMAKE_INSTALL_COMPONENT) - if(COMPONENT) - message(STATUS "Install component: \"${COMPONENT}\"") - set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") - else() - set(CMAKE_INSTALL_COMPONENT) - endif() -endif() - -# Is this installation the result of a crosscompile? -if(NOT DEFINED CMAKE_CROSSCOMPILING) - set(CMAKE_CROSSCOMPILING "FALSE") -endif() - -if(NOT CMAKE_INSTALL_LOCAL_ONLY) - # Include the install script for each subdirectory. - include("/Users/roy/projects/revolve/cpprevolve/revolve/brains/cmake_install.cmake") - include("/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/cmake_install.cmake") - -endif() - diff --git a/cpprevolve/revolve/gazebo/Makefile b/cpprevolve/revolve/gazebo/Makefile deleted file mode 100644 index 118fc6781b..0000000000 --- a/cpprevolve/revolve/gazebo/Makefile +++ /dev/null @@ -1,1238 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 3.14 - -# Default target executed when no arguments are given to make. -default_target: all - -.PHONY : default_target - -# Allow only one "make -f Makefile2" at a time, but pass parallelism. -.NOTPARALLEL: - - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - - -# A target that is always out of date. -cmake_force: - -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake - -# The command to remove a file. -RM = /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /Users/roy/projects/revolve - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /Users/roy/projects/revolve - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target list_install_components -list_install_components: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" -.PHONY : list_install_components - -# Special rule for the target list_install_components -list_install_components/fast: list_install_components - -.PHONY : list_install_components/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache - -.PHONY : rebuild_cache/fast - -# Special rule for the target install/strip -install/strip: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake -.PHONY : install/strip - -# Special rule for the target install/strip -install/strip/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake -.PHONY : install/strip/fast - -# Special rule for the target install -install: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -P cmake_install.cmake -.PHONY : install - -# Special rule for the target install -install/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -P cmake_install.cmake -.PHONY : install/fast - -# Special rule for the target install/local -install/local: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake -.PHONY : install/local - -# Special rule for the target install/local -install/local/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake -.PHONY : install/local/fast - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." - /Applications/CLion.app/Contents/bin/cmake/mac/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache - -.PHONY : edit_cache/fast - -# The main all target -all: cmake_check_build_system - cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -E cmake_progress_start /Users/roy/projects/revolve/CMakeFiles /Users/roy/projects/revolve/cpprevolve/revolve/gazebo/CMakeFiles/progress.marks - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/all - $(CMAKE_COMMAND) -E cmake_progress_start /Users/roy/projects/revolve/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/clean -.PHONY : clean - -# The main clean target -clean/fast: clean - -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -# Convenience name for target. -cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/rule: - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/rule -.PHONY : cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/rule - -# Convenience name for target. -RobotControlPlugin: cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/rule - -.PHONY : RobotControlPlugin - -# fast build rule for target. -RobotControlPlugin/fast: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build -.PHONY : RobotControlPlugin/fast - -# Convenience name for target. -cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/rule: - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/rule -.PHONY : cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/rule - -# Convenience name for target. -revolve-proto: cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/rule - -.PHONY : revolve-proto - -# fast build rule for target. -revolve-proto/fast: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build -.PHONY : revolve-proto/fast - -# Convenience name for target. -cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/rule: - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/rule -.PHONY : cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/rule - -# Convenience name for target. -WorldControlPlugin: cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/rule - -.PHONY : WorldControlPlugin - -# fast build rule for target. -WorldControlPlugin/fast: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build -.PHONY : WorldControlPlugin/fast - -# Convenience name for target. -cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/rule: - cd /Users/roy/projects/revolve && $(MAKE) -f CMakeFiles/Makefile2 cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/rule -.PHONY : cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/rule - -# Convenience name for target. -revolve-gazebo: cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/rule - -.PHONY : revolve-gazebo - -# fast build rule for target. -revolve-gazebo/fast: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build -.PHONY : revolve-gazebo/fast - -battery/Battery.o: battery/Battery.cpp.o - -.PHONY : battery/Battery.o - -# target to build an object file -battery/Battery.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/battery/Battery.cpp.o -.PHONY : battery/Battery.cpp.o - -battery/Battery.i: battery/Battery.cpp.i - -.PHONY : battery/Battery.i - -# target to preprocess a source file -battery/Battery.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/battery/Battery.cpp.i -.PHONY : battery/Battery.cpp.i - -battery/Battery.s: battery/Battery.cpp.s - -.PHONY : battery/Battery.s - -# target to generate assembly for a file -battery/Battery.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/battery/Battery.cpp.s -.PHONY : battery/Battery.cpp.s - -brains/BrainFactory.o: brains/BrainFactory.cpp.o - -.PHONY : brains/BrainFactory.o - -# target to build an object file -brains/BrainFactory.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/BrainFactory.cpp.o -.PHONY : brains/BrainFactory.cpp.o - -brains/BrainFactory.i: brains/BrainFactory.cpp.i - -.PHONY : brains/BrainFactory.i - -# target to preprocess a source file -brains/BrainFactory.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/BrainFactory.cpp.i -.PHONY : brains/BrainFactory.cpp.i - -brains/BrainFactory.s: brains/BrainFactory.cpp.s - -.PHONY : brains/BrainFactory.s - -# target to generate assembly for a file -brains/BrainFactory.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/BrainFactory.cpp.s -.PHONY : brains/BrainFactory.cpp.s - -brains/DifferentialCPG.o: brains/DifferentialCPG.cpp.o - -.PHONY : brains/DifferentialCPG.o - -# target to build an object file -brains/DifferentialCPG.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/DifferentialCPG.cpp.o -.PHONY : brains/DifferentialCPG.cpp.o - -brains/DifferentialCPG.i: brains/DifferentialCPG.cpp.i - -.PHONY : brains/DifferentialCPG.i - -# target to preprocess a source file -brains/DifferentialCPG.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/DifferentialCPG.cpp.i -.PHONY : brains/DifferentialCPG.cpp.i - -brains/DifferentialCPG.s: brains/DifferentialCPG.cpp.s - -.PHONY : brains/DifferentialCPG.s - -# target to generate assembly for a file -brains/DifferentialCPG.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/DifferentialCPG.cpp.s -.PHONY : brains/DifferentialCPG.cpp.s - -brains/Evaluator.o: brains/Evaluator.cpp.o - -.PHONY : brains/Evaluator.o - -# target to build an object file -brains/Evaluator.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/Evaluator.cpp.o -.PHONY : brains/Evaluator.cpp.o - -brains/Evaluator.i: brains/Evaluator.cpp.i - -.PHONY : brains/Evaluator.i - -# target to preprocess a source file -brains/Evaluator.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/Evaluator.cpp.i -.PHONY : brains/Evaluator.cpp.i - -brains/Evaluator.s: brains/Evaluator.cpp.s - -.PHONY : brains/Evaluator.s - -# target to generate assembly for a file -brains/Evaluator.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/Evaluator.cpp.s -.PHONY : brains/Evaluator.cpp.s - -brains/NeuralNetwork.o: brains/NeuralNetwork.cpp.o - -.PHONY : brains/NeuralNetwork.o - -# target to build an object file -brains/NeuralNetwork.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/NeuralNetwork.cpp.o -.PHONY : brains/NeuralNetwork.cpp.o - -brains/NeuralNetwork.i: brains/NeuralNetwork.cpp.i - -.PHONY : brains/NeuralNetwork.i - -# target to preprocess a source file -brains/NeuralNetwork.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/NeuralNetwork.cpp.i -.PHONY : brains/NeuralNetwork.cpp.i - -brains/NeuralNetwork.s: brains/NeuralNetwork.cpp.s - -.PHONY : brains/NeuralNetwork.s - -# target to generate assembly for a file -brains/NeuralNetwork.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/NeuralNetwork.cpp.s -.PHONY : brains/NeuralNetwork.cpp.s - -brains/RLPower.o: brains/RLPower.cpp.o - -.PHONY : brains/RLPower.o - -# target to build an object file -brains/RLPower.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/RLPower.cpp.o -.PHONY : brains/RLPower.cpp.o - -brains/RLPower.i: brains/RLPower.cpp.i - -.PHONY : brains/RLPower.i - -# target to preprocess a source file -brains/RLPower.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/RLPower.cpp.i -.PHONY : brains/RLPower.cpp.i - -brains/RLPower.s: brains/RLPower.cpp.s - -.PHONY : brains/RLPower.s - -# target to generate assembly for a file -brains/RLPower.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/RLPower.cpp.s -.PHONY : brains/RLPower.cpp.s - -brains/ThymioBrain.o: brains/ThymioBrain.cpp.o - -.PHONY : brains/ThymioBrain.o - -# target to build an object file -brains/ThymioBrain.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/ThymioBrain.cpp.o -.PHONY : brains/ThymioBrain.cpp.o - -brains/ThymioBrain.i: brains/ThymioBrain.cpp.i - -.PHONY : brains/ThymioBrain.i - -# target to preprocess a source file -brains/ThymioBrain.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/ThymioBrain.cpp.i -.PHONY : brains/ThymioBrain.cpp.i - -brains/ThymioBrain.s: brains/ThymioBrain.cpp.s - -.PHONY : brains/ThymioBrain.s - -# target to generate assembly for a file -brains/ThymioBrain.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/brains/ThymioBrain.cpp.s -.PHONY : brains/ThymioBrain.cpp.s - -motors/JointMotor.o: motors/JointMotor.cpp.o - -.PHONY : motors/JointMotor.o - -# target to build an object file -motors/JointMotor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/JointMotor.cpp.o -.PHONY : motors/JointMotor.cpp.o - -motors/JointMotor.i: motors/JointMotor.cpp.i - -.PHONY : motors/JointMotor.i - -# target to preprocess a source file -motors/JointMotor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/JointMotor.cpp.i -.PHONY : motors/JointMotor.cpp.i - -motors/JointMotor.s: motors/JointMotor.cpp.s - -.PHONY : motors/JointMotor.s - -# target to generate assembly for a file -motors/JointMotor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/JointMotor.cpp.s -.PHONY : motors/JointMotor.cpp.s - -motors/Motor.o: motors/Motor.cpp.o - -.PHONY : motors/Motor.o - -# target to build an object file -motors/Motor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/Motor.cpp.o -.PHONY : motors/Motor.cpp.o - -motors/Motor.i: motors/Motor.cpp.i - -.PHONY : motors/Motor.i - -# target to preprocess a source file -motors/Motor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/Motor.cpp.i -.PHONY : motors/Motor.cpp.i - -motors/Motor.s: motors/Motor.cpp.s - -.PHONY : motors/Motor.s - -# target to generate assembly for a file -motors/Motor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/Motor.cpp.s -.PHONY : motors/Motor.cpp.s - -motors/MotorFactory.o: motors/MotorFactory.cpp.o - -.PHONY : motors/MotorFactory.o - -# target to build an object file -motors/MotorFactory.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/MotorFactory.cpp.o -.PHONY : motors/MotorFactory.cpp.o - -motors/MotorFactory.i: motors/MotorFactory.cpp.i - -.PHONY : motors/MotorFactory.i - -# target to preprocess a source file -motors/MotorFactory.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/MotorFactory.cpp.i -.PHONY : motors/MotorFactory.cpp.i - -motors/MotorFactory.s: motors/MotorFactory.cpp.s - -.PHONY : motors/MotorFactory.s - -# target to generate assembly for a file -motors/MotorFactory.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/MotorFactory.cpp.s -.PHONY : motors/MotorFactory.cpp.s - -motors/PositionMotor.o: motors/PositionMotor.cpp.o - -.PHONY : motors/PositionMotor.o - -# target to build an object file -motors/PositionMotor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/PositionMotor.cpp.o -.PHONY : motors/PositionMotor.cpp.o - -motors/PositionMotor.i: motors/PositionMotor.cpp.i - -.PHONY : motors/PositionMotor.i - -# target to preprocess a source file -motors/PositionMotor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/PositionMotor.cpp.i -.PHONY : motors/PositionMotor.cpp.i - -motors/PositionMotor.s: motors/PositionMotor.cpp.s - -.PHONY : motors/PositionMotor.s - -# target to generate assembly for a file -motors/PositionMotor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/PositionMotor.cpp.s -.PHONY : motors/PositionMotor.cpp.s - -motors/VelocityMotor.o: motors/VelocityMotor.cpp.o - -.PHONY : motors/VelocityMotor.o - -# target to build an object file -motors/VelocityMotor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/VelocityMotor.cpp.o -.PHONY : motors/VelocityMotor.cpp.o - -motors/VelocityMotor.i: motors/VelocityMotor.cpp.i - -.PHONY : motors/VelocityMotor.i - -# target to preprocess a source file -motors/VelocityMotor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/VelocityMotor.cpp.i -.PHONY : motors/VelocityMotor.cpp.i - -motors/VelocityMotor.s: motors/VelocityMotor.cpp.s - -.PHONY : motors/VelocityMotor.s - -# target to generate assembly for a file -motors/VelocityMotor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/motors/VelocityMotor.cpp.s -.PHONY : motors/VelocityMotor.cpp.s - -plugin/RobotController.o: plugin/RobotController.cpp.o - -.PHONY : plugin/RobotController.o - -# target to build an object file -plugin/RobotController.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/RobotController.cpp.o -.PHONY : plugin/RobotController.cpp.o - -plugin/RobotController.i: plugin/RobotController.cpp.i - -.PHONY : plugin/RobotController.i - -# target to preprocess a source file -plugin/RobotController.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/RobotController.cpp.i -.PHONY : plugin/RobotController.cpp.i - -plugin/RobotController.s: plugin/RobotController.cpp.s - -.PHONY : plugin/RobotController.s - -# target to generate assembly for a file -plugin/RobotController.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/RobotController.cpp.s -.PHONY : plugin/RobotController.cpp.s - -plugin/WorldController.o: plugin/WorldController.cpp.o - -.PHONY : plugin/WorldController.o - -# target to build an object file -plugin/WorldController.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/WorldController.cpp.o -.PHONY : plugin/WorldController.cpp.o - -plugin/WorldController.i: plugin/WorldController.cpp.i - -.PHONY : plugin/WorldController.i - -# target to preprocess a source file -plugin/WorldController.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/WorldController.cpp.i -.PHONY : plugin/WorldController.cpp.i - -plugin/WorldController.s: plugin/WorldController.cpp.s - -.PHONY : plugin/WorldController.s - -# target to generate assembly for a file -plugin/WorldController.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/plugin/WorldController.cpp.s -.PHONY : plugin/WorldController.cpp.s - -plugin/register_robot_plugin.o: plugin/register_robot_plugin.cpp.o - -.PHONY : plugin/register_robot_plugin.o - -# target to build an object file -plugin/register_robot_plugin.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/plugin/register_robot_plugin.cpp.o -.PHONY : plugin/register_robot_plugin.cpp.o - -plugin/register_robot_plugin.i: plugin/register_robot_plugin.cpp.i - -.PHONY : plugin/register_robot_plugin.i - -# target to preprocess a source file -plugin/register_robot_plugin.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/plugin/register_robot_plugin.cpp.i -.PHONY : plugin/register_robot_plugin.cpp.i - -plugin/register_robot_plugin.s: plugin/register_robot_plugin.cpp.s - -.PHONY : plugin/register_robot_plugin.s - -# target to generate assembly for a file -plugin/register_robot_plugin.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/RobotControlPlugin.dir/plugin/register_robot_plugin.cpp.s -.PHONY : plugin/register_robot_plugin.cpp.s - -plugin/register_world_plugin.o: plugin/register_world_plugin.cpp.o - -.PHONY : plugin/register_world_plugin.o - -# target to build an object file -plugin/register_world_plugin.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/plugin/register_world_plugin.cpp.o -.PHONY : plugin/register_world_plugin.cpp.o - -plugin/register_world_plugin.i: plugin/register_world_plugin.cpp.i - -.PHONY : plugin/register_world_plugin.i - -# target to preprocess a source file -plugin/register_world_plugin.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/plugin/register_world_plugin.cpp.i -.PHONY : plugin/register_world_plugin.cpp.i - -plugin/register_world_plugin.s: plugin/register_world_plugin.cpp.s - -.PHONY : plugin/register_world_plugin.s - -# target to generate assembly for a file -plugin/register_world_plugin.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/WorldControlPlugin.dir/plugin/register_world_plugin.cpp.s -.PHONY : plugin/register_world_plugin.cpp.s - -revolve/msgs/body.pb.o: revolve/msgs/body.pb.cc.o - -.PHONY : revolve/msgs/body.pb.o - -# target to build an object file -revolve/msgs/body.pb.cc.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/body.pb.cc.o -.PHONY : revolve/msgs/body.pb.cc.o - -revolve/msgs/body.pb.i: revolve/msgs/body.pb.cc.i - -.PHONY : revolve/msgs/body.pb.i - -# target to preprocess a source file -revolve/msgs/body.pb.cc.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/body.pb.cc.i -.PHONY : revolve/msgs/body.pb.cc.i - -revolve/msgs/body.pb.s: revolve/msgs/body.pb.cc.s - -.PHONY : revolve/msgs/body.pb.s - -# target to generate assembly for a file -revolve/msgs/body.pb.cc.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/body.pb.cc.s -.PHONY : revolve/msgs/body.pb.cc.s - -revolve/msgs/model_inserted.pb.o: revolve/msgs/model_inserted.pb.cc.o - -.PHONY : revolve/msgs/model_inserted.pb.o - -# target to build an object file -revolve/msgs/model_inserted.pb.cc.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/model_inserted.pb.cc.o -.PHONY : revolve/msgs/model_inserted.pb.cc.o - -revolve/msgs/model_inserted.pb.i: revolve/msgs/model_inserted.pb.cc.i - -.PHONY : revolve/msgs/model_inserted.pb.i - -# target to preprocess a source file -revolve/msgs/model_inserted.pb.cc.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/model_inserted.pb.cc.i -.PHONY : revolve/msgs/model_inserted.pb.cc.i - -revolve/msgs/model_inserted.pb.s: revolve/msgs/model_inserted.pb.cc.s - -.PHONY : revolve/msgs/model_inserted.pb.s - -# target to generate assembly for a file -revolve/msgs/model_inserted.pb.cc.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/model_inserted.pb.cc.s -.PHONY : revolve/msgs/model_inserted.pb.cc.s - -revolve/msgs/neural_net.pb.o: revolve/msgs/neural_net.pb.cc.o - -.PHONY : revolve/msgs/neural_net.pb.o - -# target to build an object file -revolve/msgs/neural_net.pb.cc.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/neural_net.pb.cc.o -.PHONY : revolve/msgs/neural_net.pb.cc.o - -revolve/msgs/neural_net.pb.i: revolve/msgs/neural_net.pb.cc.i - -.PHONY : revolve/msgs/neural_net.pb.i - -# target to preprocess a source file -revolve/msgs/neural_net.pb.cc.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/neural_net.pb.cc.i -.PHONY : revolve/msgs/neural_net.pb.cc.i - -revolve/msgs/neural_net.pb.s: revolve/msgs/neural_net.pb.cc.s - -.PHONY : revolve/msgs/neural_net.pb.s - -# target to generate assembly for a file -revolve/msgs/neural_net.pb.cc.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/neural_net.pb.cc.s -.PHONY : revolve/msgs/neural_net.pb.cc.s - -revolve/msgs/parameter.pb.o: revolve/msgs/parameter.pb.cc.o - -.PHONY : revolve/msgs/parameter.pb.o - -# target to build an object file -revolve/msgs/parameter.pb.cc.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/parameter.pb.cc.o -.PHONY : revolve/msgs/parameter.pb.cc.o - -revolve/msgs/parameter.pb.i: revolve/msgs/parameter.pb.cc.i - -.PHONY : revolve/msgs/parameter.pb.i - -# target to preprocess a source file -revolve/msgs/parameter.pb.cc.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/parameter.pb.cc.i -.PHONY : revolve/msgs/parameter.pb.cc.i - -revolve/msgs/parameter.pb.s: revolve/msgs/parameter.pb.cc.s - -.PHONY : revolve/msgs/parameter.pb.s - -# target to generate assembly for a file -revolve/msgs/parameter.pb.cc.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/parameter.pb.cc.s -.PHONY : revolve/msgs/parameter.pb.cc.s - -revolve/msgs/robot.pb.o: revolve/msgs/robot.pb.cc.o - -.PHONY : revolve/msgs/robot.pb.o - -# target to build an object file -revolve/msgs/robot.pb.cc.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot.pb.cc.o -.PHONY : revolve/msgs/robot.pb.cc.o - -revolve/msgs/robot.pb.i: revolve/msgs/robot.pb.cc.i - -.PHONY : revolve/msgs/robot.pb.i - -# target to preprocess a source file -revolve/msgs/robot.pb.cc.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot.pb.cc.i -.PHONY : revolve/msgs/robot.pb.cc.i - -revolve/msgs/robot.pb.s: revolve/msgs/robot.pb.cc.s - -.PHONY : revolve/msgs/robot.pb.s - -# target to generate assembly for a file -revolve/msgs/robot.pb.cc.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot.pb.cc.s -.PHONY : revolve/msgs/robot.pb.cc.s - -revolve/msgs/robot_states.pb.o: revolve/msgs/robot_states.pb.cc.o - -.PHONY : revolve/msgs/robot_states.pb.o - -# target to build an object file -revolve/msgs/robot_states.pb.cc.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot_states.pb.cc.o -.PHONY : revolve/msgs/robot_states.pb.cc.o - -revolve/msgs/robot_states.pb.i: revolve/msgs/robot_states.pb.cc.i - -.PHONY : revolve/msgs/robot_states.pb.i - -# target to preprocess a source file -revolve/msgs/robot_states.pb.cc.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot_states.pb.cc.i -.PHONY : revolve/msgs/robot_states.pb.cc.i - -revolve/msgs/robot_states.pb.s: revolve/msgs/robot_states.pb.cc.s - -.PHONY : revolve/msgs/robot_states.pb.s - -# target to generate assembly for a file -revolve/msgs/robot_states.pb.cc.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/robot_states.pb.cc.s -.PHONY : revolve/msgs/robot_states.pb.cc.s - -revolve/msgs/sdf_body_analyze.pb.o: revolve/msgs/sdf_body_analyze.pb.cc.o - -.PHONY : revolve/msgs/sdf_body_analyze.pb.o - -# target to build an object file -revolve/msgs/sdf_body_analyze.pb.cc.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/sdf_body_analyze.pb.cc.o -.PHONY : revolve/msgs/sdf_body_analyze.pb.cc.o - -revolve/msgs/sdf_body_analyze.pb.i: revolve/msgs/sdf_body_analyze.pb.cc.i - -.PHONY : revolve/msgs/sdf_body_analyze.pb.i - -# target to preprocess a source file -revolve/msgs/sdf_body_analyze.pb.cc.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/sdf_body_analyze.pb.cc.i -.PHONY : revolve/msgs/sdf_body_analyze.pb.cc.i - -revolve/msgs/sdf_body_analyze.pb.s: revolve/msgs/sdf_body_analyze.pb.cc.s - -.PHONY : revolve/msgs/sdf_body_analyze.pb.s - -# target to generate assembly for a file -revolve/msgs/sdf_body_analyze.pb.cc.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/sdf_body_analyze.pb.cc.s -.PHONY : revolve/msgs/sdf_body_analyze.pb.cc.s - -revolve/msgs/spline_policy.pb.o: revolve/msgs/spline_policy.pb.cc.o - -.PHONY : revolve/msgs/spline_policy.pb.o - -# target to build an object file -revolve/msgs/spline_policy.pb.cc.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/spline_policy.pb.cc.o -.PHONY : revolve/msgs/spline_policy.pb.cc.o - -revolve/msgs/spline_policy.pb.i: revolve/msgs/spline_policy.pb.cc.i - -.PHONY : revolve/msgs/spline_policy.pb.i - -# target to preprocess a source file -revolve/msgs/spline_policy.pb.cc.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/spline_policy.pb.cc.i -.PHONY : revolve/msgs/spline_policy.pb.cc.i - -revolve/msgs/spline_policy.pb.s: revolve/msgs/spline_policy.pb.cc.s - -.PHONY : revolve/msgs/spline_policy.pb.s - -# target to generate assembly for a file -revolve/msgs/spline_policy.pb.cc.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-proto.dir/revolve/msgs/spline_policy.pb.cc.s -.PHONY : revolve/msgs/spline_policy.pb.cc.s - -sensors/BatterySensor.o: sensors/BatterySensor.cpp.o - -.PHONY : sensors/BatterySensor.o - -# target to build an object file -sensors/BatterySensor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/BatterySensor.cpp.o -.PHONY : sensors/BatterySensor.cpp.o - -sensors/BatterySensor.i: sensors/BatterySensor.cpp.i - -.PHONY : sensors/BatterySensor.i - -# target to preprocess a source file -sensors/BatterySensor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/BatterySensor.cpp.i -.PHONY : sensors/BatterySensor.cpp.i - -sensors/BatterySensor.s: sensors/BatterySensor.cpp.s - -.PHONY : sensors/BatterySensor.s - -# target to generate assembly for a file -sensors/BatterySensor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/BatterySensor.cpp.s -.PHONY : sensors/BatterySensor.cpp.s - -sensors/ImuSensor.o: sensors/ImuSensor.cpp.o - -.PHONY : sensors/ImuSensor.o - -# target to build an object file -sensors/ImuSensor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/ImuSensor.cpp.o -.PHONY : sensors/ImuSensor.cpp.o - -sensors/ImuSensor.i: sensors/ImuSensor.cpp.i - -.PHONY : sensors/ImuSensor.i - -# target to preprocess a source file -sensors/ImuSensor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/ImuSensor.cpp.i -.PHONY : sensors/ImuSensor.cpp.i - -sensors/ImuSensor.s: sensors/ImuSensor.cpp.s - -.PHONY : sensors/ImuSensor.s - -# target to generate assembly for a file -sensors/ImuSensor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/ImuSensor.cpp.s -.PHONY : sensors/ImuSensor.cpp.s - -sensors/LightSensor.o: sensors/LightSensor.cpp.o - -.PHONY : sensors/LightSensor.o - -# target to build an object file -sensors/LightSensor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/LightSensor.cpp.o -.PHONY : sensors/LightSensor.cpp.o - -sensors/LightSensor.i: sensors/LightSensor.cpp.i - -.PHONY : sensors/LightSensor.i - -# target to preprocess a source file -sensors/LightSensor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/LightSensor.cpp.i -.PHONY : sensors/LightSensor.cpp.i - -sensors/LightSensor.s: sensors/LightSensor.cpp.s - -.PHONY : sensors/LightSensor.s - -# target to generate assembly for a file -sensors/LightSensor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/LightSensor.cpp.s -.PHONY : sensors/LightSensor.cpp.s - -sensors/PointIntensitySensor.o: sensors/PointIntensitySensor.cpp.o - -.PHONY : sensors/PointIntensitySensor.o - -# target to build an object file -sensors/PointIntensitySensor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/PointIntensitySensor.cpp.o -.PHONY : sensors/PointIntensitySensor.cpp.o - -sensors/PointIntensitySensor.i: sensors/PointIntensitySensor.cpp.i - -.PHONY : sensors/PointIntensitySensor.i - -# target to preprocess a source file -sensors/PointIntensitySensor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/PointIntensitySensor.cpp.i -.PHONY : sensors/PointIntensitySensor.cpp.i - -sensors/PointIntensitySensor.s: sensors/PointIntensitySensor.cpp.s - -.PHONY : sensors/PointIntensitySensor.s - -# target to generate assembly for a file -sensors/PointIntensitySensor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/PointIntensitySensor.cpp.s -.PHONY : sensors/PointIntensitySensor.cpp.s - -sensors/Sensor.o: sensors/Sensor.cpp.o - -.PHONY : sensors/Sensor.o - -# target to build an object file -sensors/Sensor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/Sensor.cpp.o -.PHONY : sensors/Sensor.cpp.o - -sensors/Sensor.i: sensors/Sensor.cpp.i - -.PHONY : sensors/Sensor.i - -# target to preprocess a source file -sensors/Sensor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/Sensor.cpp.i -.PHONY : sensors/Sensor.cpp.i - -sensors/Sensor.s: sensors/Sensor.cpp.s - -.PHONY : sensors/Sensor.s - -# target to generate assembly for a file -sensors/Sensor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/Sensor.cpp.s -.PHONY : sensors/Sensor.cpp.s - -sensors/SensorFactory.o: sensors/SensorFactory.cpp.o - -.PHONY : sensors/SensorFactory.o - -# target to build an object file -sensors/SensorFactory.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/SensorFactory.cpp.o -.PHONY : sensors/SensorFactory.cpp.o - -sensors/SensorFactory.i: sensors/SensorFactory.cpp.i - -.PHONY : sensors/SensorFactory.i - -# target to preprocess a source file -sensors/SensorFactory.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/SensorFactory.cpp.i -.PHONY : sensors/SensorFactory.cpp.i - -sensors/SensorFactory.s: sensors/SensorFactory.cpp.s - -.PHONY : sensors/SensorFactory.s - -# target to generate assembly for a file -sensors/SensorFactory.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/SensorFactory.cpp.s -.PHONY : sensors/SensorFactory.cpp.s - -sensors/TouchSensor.o: sensors/TouchSensor.cpp.o - -.PHONY : sensors/TouchSensor.o - -# target to build an object file -sensors/TouchSensor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/TouchSensor.cpp.o -.PHONY : sensors/TouchSensor.cpp.o - -sensors/TouchSensor.i: sensors/TouchSensor.cpp.i - -.PHONY : sensors/TouchSensor.i - -# target to preprocess a source file -sensors/TouchSensor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/TouchSensor.cpp.i -.PHONY : sensors/TouchSensor.cpp.i - -sensors/TouchSensor.s: sensors/TouchSensor.cpp.s - -.PHONY : sensors/TouchSensor.s - -# target to generate assembly for a file -sensors/TouchSensor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/TouchSensor.cpp.s -.PHONY : sensors/TouchSensor.cpp.s - -sensors/VirtualSensor.o: sensors/VirtualSensor.cpp.o - -.PHONY : sensors/VirtualSensor.o - -# target to build an object file -sensors/VirtualSensor.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/VirtualSensor.cpp.o -.PHONY : sensors/VirtualSensor.cpp.o - -sensors/VirtualSensor.i: sensors/VirtualSensor.cpp.i - -.PHONY : sensors/VirtualSensor.i - -# target to preprocess a source file -sensors/VirtualSensor.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/VirtualSensor.cpp.i -.PHONY : sensors/VirtualSensor.cpp.i - -sensors/VirtualSensor.s: sensors/VirtualSensor.cpp.s - -.PHONY : sensors/VirtualSensor.s - -# target to generate assembly for a file -sensors/VirtualSensor.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/sensors/VirtualSensor.cpp.s -.PHONY : sensors/VirtualSensor.cpp.s - -util/YamlBodyParser.o: util/YamlBodyParser.cpp.o - -.PHONY : util/YamlBodyParser.o - -# target to build an object file -util/YamlBodyParser.cpp.o: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/util/YamlBodyParser.cpp.o -.PHONY : util/YamlBodyParser.cpp.o - -util/YamlBodyParser.i: util/YamlBodyParser.cpp.i - -.PHONY : util/YamlBodyParser.i - -# target to preprocess a source file -util/YamlBodyParser.cpp.i: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/util/YamlBodyParser.cpp.i -.PHONY : util/YamlBodyParser.cpp.i - -util/YamlBodyParser.s: util/YamlBodyParser.cpp.s - -.PHONY : util/YamlBodyParser.s - -# target to generate assembly for a file -util/YamlBodyParser.cpp.s: - cd /Users/roy/projects/revolve && $(MAKE) -f cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/build.make cpprevolve/revolve/gazebo/CMakeFiles/revolve-gazebo.dir/util/YamlBodyParser.cpp.s -.PHONY : util/YamlBodyParser.cpp.s - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... list_install_components" - @echo "... rebuild_cache" - @echo "... install/strip" - @echo "... install" - @echo "... RobotControlPlugin" - @echo "... install/local" - @echo "... revolve-proto" - @echo "... WorldControlPlugin" - @echo "... revolve-gazebo" - @echo "... edit_cache" - @echo "... battery/Battery.o" - @echo "... battery/Battery.i" - @echo "... battery/Battery.s" - @echo "... brains/BrainFactory.o" - @echo "... brains/BrainFactory.i" - @echo "... brains/BrainFactory.s" - @echo "... brains/DifferentialCPG.o" - @echo "... brains/DifferentialCPG.i" - @echo "... brains/DifferentialCPG.s" - @echo "... brains/Evaluator.o" - @echo "... brains/Evaluator.i" - @echo "... brains/Evaluator.s" - @echo "... brains/NeuralNetwork.o" - @echo "... brains/NeuralNetwork.i" - @echo "... brains/NeuralNetwork.s" - @echo "... brains/RLPower.o" - @echo "... brains/RLPower.i" - @echo "... brains/RLPower.s" - @echo "... brains/ThymioBrain.o" - @echo "... brains/ThymioBrain.i" - @echo "... brains/ThymioBrain.s" - @echo "... motors/JointMotor.o" - @echo "... motors/JointMotor.i" - @echo "... motors/JointMotor.s" - @echo "... motors/Motor.o" - @echo "... motors/Motor.i" - @echo "... motors/Motor.s" - @echo "... motors/MotorFactory.o" - @echo "... motors/MotorFactory.i" - @echo "... motors/MotorFactory.s" - @echo "... motors/PositionMotor.o" - @echo "... motors/PositionMotor.i" - @echo "... motors/PositionMotor.s" - @echo "... motors/VelocityMotor.o" - @echo "... motors/VelocityMotor.i" - @echo "... motors/VelocityMotor.s" - @echo "... plugin/RobotController.o" - @echo "... plugin/RobotController.i" - @echo "... plugin/RobotController.s" - @echo "... plugin/WorldController.o" - @echo "... plugin/WorldController.i" - @echo "... plugin/WorldController.s" - @echo "... plugin/register_robot_plugin.o" - @echo "... plugin/register_robot_plugin.i" - @echo "... plugin/register_robot_plugin.s" - @echo "... plugin/register_world_plugin.o" - @echo "... plugin/register_world_plugin.i" - @echo "... plugin/register_world_plugin.s" - @echo "... revolve/msgs/body.pb.o" - @echo "... revolve/msgs/body.pb.i" - @echo "... revolve/msgs/body.pb.s" - @echo "... revolve/msgs/model_inserted.pb.o" - @echo "... revolve/msgs/model_inserted.pb.i" - @echo "... revolve/msgs/model_inserted.pb.s" - @echo "... revolve/msgs/neural_net.pb.o" - @echo "... revolve/msgs/neural_net.pb.i" - @echo "... revolve/msgs/neural_net.pb.s" - @echo "... revolve/msgs/parameter.pb.o" - @echo "... revolve/msgs/parameter.pb.i" - @echo "... revolve/msgs/parameter.pb.s" - @echo "... revolve/msgs/robot.pb.o" - @echo "... revolve/msgs/robot.pb.i" - @echo "... revolve/msgs/robot.pb.s" - @echo "... revolve/msgs/robot_states.pb.o" - @echo "... revolve/msgs/robot_states.pb.i" - @echo "... revolve/msgs/robot_states.pb.s" - @echo "... revolve/msgs/sdf_body_analyze.pb.o" - @echo "... revolve/msgs/sdf_body_analyze.pb.i" - @echo "... revolve/msgs/sdf_body_analyze.pb.s" - @echo "... revolve/msgs/spline_policy.pb.o" - @echo "... revolve/msgs/spline_policy.pb.i" - @echo "... revolve/msgs/spline_policy.pb.s" - @echo "... sensors/BatterySensor.o" - @echo "... sensors/BatterySensor.i" - @echo "... sensors/BatterySensor.s" - @echo "... sensors/ImuSensor.o" - @echo "... sensors/ImuSensor.i" - @echo "... sensors/ImuSensor.s" - @echo "... sensors/LightSensor.o" - @echo "... sensors/LightSensor.i" - @echo "... sensors/LightSensor.s" - @echo "... sensors/PointIntensitySensor.o" - @echo "... sensors/PointIntensitySensor.i" - @echo "... sensors/PointIntensitySensor.s" - @echo "... sensors/Sensor.o" - @echo "... sensors/Sensor.i" - @echo "... sensors/Sensor.s" - @echo "... sensors/SensorFactory.o" - @echo "... sensors/SensorFactory.i" - @echo "... sensors/SensorFactory.s" - @echo "... sensors/TouchSensor.o" - @echo "... sensors/TouchSensor.i" - @echo "... sensors/TouchSensor.s" - @echo "... sensors/VirtualSensor.o" - @echo "... sensors/VirtualSensor.i" - @echo "... sensors/VirtualSensor.s" - @echo "... util/YamlBodyParser.o" - @echo "... util/YamlBodyParser.i" - @echo "... util/YamlBodyParser.s" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - cd /Users/roy/projects/revolve && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/cpprevolve/revolve/gazebo/battery/Battery.cpp b/cpprevolve/revolve/gazebo/battery/Battery.cpp index 58999d72cb..09617d92e6 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.cpp +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -19,10 +19,11 @@ void Battery::Update(double global_time, double delta_time) 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() << std::endl; + 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; -// std::cout << this->time_init<< std::endl; } diff --git a/cpprevolve/revolve/gazebo/battery/battery_chart.py b/cpprevolve/revolve/gazebo/battery/battery_chart.py deleted file mode 100644 index 7928912187..0000000000 --- a/cpprevolve/revolve/gazebo/battery/battery_chart.py +++ /dev/null @@ -1,123 +0,0 @@ -import matplotlib -import matplotlib.pyplot as plt -import numpy as np -import pandas as pd -from os import listdir -from os.path import isfile, join - - -from scipy.ndimage.filters import gaussian_filter1d - - -def average(n, lst): - res = [] - for i in range(len(lst)//n): - res.append(sum(lst[i*n:(i+1)*n])/n) - return res - - -def draw_graph(name, x, y): - plt.title('Watts Used Plot ' + name) - # plt.plot(x, y) - plt.plot(x, gaussian_filter1d(y, sigma=100)) - plt.plot(x, gaussian_filter1d(y, sigma=300)) - plt.plot(x, gaussian_filter1d(y, sigma=500)) - plt.plot(x, gaussian_filter1d(y, sigma=1000)) - # plt.vlines(range(0,int(x[-1]),60), -0.13, -0.225, colors='k', linestyles='solid', label='') - plt.ylabel('amount of watts used on current time\'s update') - plt.xlabel('time (s)') - plt.show() - -def draw_graphs(name, x, y1, y2): - plt.title('Current Charge Plot ' + name) - plt.subplot(2, 1, 1) - # plt.vlines(range(0,int(x[-1]),60), -0.13, -0.17, colors='k', linestyles='solid', label='') - plt.plot(x, gaussian_filter1d(y1, sigma=100)) - plt.plot(x, gaussian_filter1d(y1, sigma=300)) - plt.plot(x, gaussian_filter1d(y1, sigma=500)) - plt.plot(x, gaussian_filter1d(y1, sigma=1000)) - plt.ylabel('energy used at instance (joules)') - plt.subplot(2, 1, 2) - plt.plot(x, y2) - - # plt.vlines(range(0,int(x[-1]),60), 0, y2[-1], colors='k', linestyles='solid', label='') - plt.xlabel('time (s)') - plt.ylabel('total energy used (joules)') - plt.show() - -def main(): - # matplotlib.use('MacOSX') - - COLUMNS = [ - "global_time", - "watts_used", - "current_charge" - ] - - mypath = "data/babyB/" - onlyfiles = [f for f in listdir(mypath) if f.endswith('.txt')] - - n_iter = 120 - robot_average = None - for file_name in onlyfiles: - global_time = [] - watts_used = [] - current_charge = [] - - draw30 = False # plot for first 30 seconds - drawIter = True # plot for each iteration (60s) - i = 1 - temp_w = [] - temp_c = [] - temp_diff = [] - - - - with open(mypath + file_name, 'r') as file: - for line in file: - data = line.strip('\n').split(' ') - global_time.append(float(data[0])) - watts_used.append(float(data[1])) - current_charge.append(float(data[2])) - - if drawIter: - temp_w.append(float(data[1])) - temp_c.append(float(data[2])) - if float(data[0]) > i * 60 : - temp_diff.append(temp_c[0]-temp_c[-1]) - # draw_graphs(file_name +" iteration: "+ str(i),range(len(temp_c)), temp_w, temp_c) - # draw_graph(file_name +" iteration: "+ str(i),range(len(temp_c)), temp_w) - i += 1 - temp_c.clear() - temp_w.clear() - - if float(data[0]) > 500 and draw30: - draw_graphs(file_name, global_time, watts_used, current_charge) - draw30 = False - # draw_graph(file_name, global_time, watts_used) - # draw_graphs(file_name, global_time, watts_used, current_charge) - - - plt.plot(range(1, 121), temp_diff,marker='x') - # plt.axis([0,120,6.4,14]) - plt.xlabel('iteration number') - plt.ylabel('total energy used (joules)') - plt.show() - - if robot_average is None: - robot_average = np.array(temp_diff) - else: - robot_average += np.array(temp_diff) - - - - robot_average /= 10 - plt.title("the average of joules used per iteration of all robots") - plt.plot(range(1, 121), robot_average ,marker='x') - # plt.axis([0,120,6.4,14]) - plt.xlabel('iteration number') - plt.ylabel('total energy used (joules)') - plt.show() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyA/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyB/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/babyC/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko12/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko17/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/gecko7/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider13/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider17/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt b/cpprevolve/revolve/gazebo/battery/data/spider9/delete_this.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 4c60ad5c6b..5fda587077 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -397,7 +397,7 @@ struct DifferentialCPG::evaluation_function{ // babyB:22, // babyC:32, // one+:12 - BO_PARAM(size_t, dim_in, 0); // CHANGETHIS + BO_PARAM(size_t, dim_in, 13); // CHANGETHIS // number of dimensions of the fitness BO_PARAM(size_t, dim_out, 1); @@ -668,25 +668,6 @@ void DifferentialCPG::bo_step(){ // Optimize. Pass dummy evaluation function and observations . - if (this->robot->GetName() == "example_spider") - const int DIM_IN = 18; - else if (this->robot->GetName() == "example_spider13") - const int DIM_IN = 26; - else if (this->robot->GetName() == "example_spider17") - const int DIM_IN = 34; - else if (this->robot->GetName() == "example_gecko7") - const int DIM_IN = 13; - else if (this->robot->GetName() == "example_gecko12") - const int DIM_IN = 23; - else if (this->robot->GetName() == "example_gecko17") - const int DIM_IN = 33; - else if (this->robot->GetName() == "example_babyA") - const int DIM_IN = 16; - else if (this->robot->GetName() == "example_babyB") - const int DIM_IN = 22; - else if (this->robot->GetName() == "example_babyC") - const int DIM_IN = 32; - boptimizer.optimize(DifferentialCPG::evaluation_function(), this->samples, this->observations); @@ -864,7 +845,7 @@ void DifferentialCPG::Update( { std::cout << std::endl << "I am finished " << std::endl; } - std::exit(0); +// std::exit(0); } // Evaluation policy here diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp index 71fcd68b89..03e1864ecf 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp @@ -51,7 +51,7 @@ Evaluator::Evaluator(std::shared_ptr<::revolve::gazebo::Battery> battery, this->current_position_.Reset(); this->previous_position_.Reset(); this->start_position_.Reset(); - this->locomotion_type = "battery"; // {directed, gait, battery} // STEP 3 + this->locomotion_type = "directed"; // {directed, gait, battery} // STEP 3 this->path_length = 0.0; } @@ -364,10 +364,10 @@ double Evaluator::Fitness() // ************************ + // + // + // f = P_initial - //f = (D_p/D) * b -[ (1 - (D_p/D) ) * (b_t + (len * size(13-34) * P) ) ] - fitness_value = (distance_projection/shortest_distance) * initial_battery - - ((1 - (distance_projection/shortest_distance) ) * (battery_time + (this->path_length * robot_size * -power_used))); // std::cout << "b: " << initial_battery << "\n"; // std::cout << "size: " << robot_size << "\n"; @@ -384,6 +384,7 @@ double Evaluator::Fitness() std::cout << "fitness: " << fitness_value << "\n"; this->battery_->current_charge = 0; /// changing its charge to 0 to simulate the reseting of the robot } + exit(0); return fitness_value; } diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index 58b9e19cd0..8f01d9a4b0 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -135,9 +135,7 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) if (this->battery_) { ::gazebo::physics::JointWrench jointWrench = this->joint_->GetForceTorque(0); -// std::cout << "dot (local): " << this->joint_->LocalAxis(0).Dot(jointWrench.body1Torque) * cmd << '\t'; // dot product of torque and local axis -// std::cout << "dot (global): " << this->joint_->GlobalAxis(0).Dot(jointWrench.body1Torque) * cmd << std::endl; // TODO find which axis to use local or global // TODO check the watt if it should be positive or negative // TODO change this for now im using the absolute value of the watt so it always decreases from the joint movements diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index eef9e16a4a..b8fa56af29 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -253,7 +253,5 @@ void RobotController::LoadBattery(const sdf::ElementPtr _sdf) this->battery_->ResetVoltage(); this->battery_->robot_name = this->model_->GetName(); -// this->battery_->SetUpdateFunc([](const ::gazebo::common::BatteryPtr &battery) -> double { -// }); } } diff --git a/experiments/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index bd7b441dad..2b1c04832c 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -17,36 +17,42 @@ from glob import glob from joblib import Parallel, delayed - # Parameters min_lines = 1490 run_gazebo = False -n_runs = 15 # Naar 20 -n_jobs = 4 +n_runs = 20 # Naar 20 +n_jobs = 1 my_yaml_path = "experiments/bo_learner/yaml/" -yaml_model = "spider9.yaml" # CHANGETHIS ! +yaml_model = "gecko7.yaml" # CHANGETHIS ! manager = "experiments/bo_learner/manager.py" python_interpreter = "~/projects/revolve/.venv/bin/python3" + + + + search_space = { - # 'load_brain': ["/Users/lan/projects/revolve/output/cpg_bo/one/main_1560413639/0/0/best_brain.txt"], - 'evaluation_rate': [60], - 'init_method': ["LHS"], - 'verbose': [1], - 'kernel_l': [0.1], - 'acqui_ucb_alpha': [1.0], - 'n_learning_iterations': [1450], - 'n_init_samples': [50], + 'load_brain': ["'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.31482.txt'", + "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.33971.txt'", + "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.34137.txt'", + "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.44568.txt'"] + # 'evaluation_rate': [60], + # 'init_method': ["LHS"], + # 'verbose': [1], + # 'kernel_l': [0.1], + # 'acqui_ucb_alpha': [1.0], + # 'n_learning_iterations': [1450], + # 'n_init_samples': [50], # 'kernel_l': [0.2], # 'acqui_ucb_alpha': [3.0], # 'n_learning_iterations': [950], # 'n_init_samples': [20], } -print(search_space) +print('search_space:', search_space) # You don't have to change this my_sub_directory = "yaml_temp/" output_path = "output/cpg_bo/main_" + str(round(time.time())) + "/" -start_port = 13000 # STEP 6 +start_port = 12000 # STEP 6 finished = False # Make in revolve/build to allow runs from terminal @@ -156,32 +162,32 @@ def run(i, sub_directory, model, params): experiments = [dict(zip(keys, v)) for v in itertools.product(*values)] # PASTE THE EXPERIMENTS HERE, IN THE FORMAT SHOWN BELOW - experiments = [ - {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0} # STEP 4 - - # # BASE RUN - # {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - # {'init_method': "LHS", 'kernel_l': 0.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - # {'init_method': "LHS", 'kernel_l': 1.0, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - # {'init_method': "LHS", 'kernel_l': 1.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - # - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.1, 'acqui_ucb_alpha': 0.5}, - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.2, 'acqui_ucb_alpha': 0.5}, - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.5, 'acqui_ucb_alpha': 0.5}, - # # BASE RUN - # - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.1}, - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.2}, - # # BASE RUN - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.0}, - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.5}, - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 2.0}, - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0}, - # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 4.0}, - # - # {'init_method': "RS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, - # BASE RUN - ] + # experiments = [ + # # {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0} # STEP 4 + # # + # # # # BASE RUN + # # # {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 0.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 1.0, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 1.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.1, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.2, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.5, 'acqui_ucb_alpha': 0.5}, + # # # # BASE RUN + # # # + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.1}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.2}, + # # # # BASE RUN + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.0}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.5}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 2.0}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 4.0}, + # # # + # # # {'init_method': "RS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # BASE RUN + # # ] # 'kernel_l': [0.02, 0.05, 0.1, 0.2], # 'acqui_ucb_alpha': [0.1, 0.3, 0.5, 1.0], unique_experiments = experiments @@ -339,4 +345,4 @@ def run(i, sub_directory, model, params): params_string =[] for key, value in iter(search_space.items()): params_string += [str(key) + ": " + str(value)] - write_file(output_path + "search_space.txt", params_string) \ No newline at end of file + write_file(output_path + "search_space.txt", params_string) diff --git a/experiments/bo_learner/yaml/babyA.yaml b/experiments/bo_learner/yaml/babyA.yaml index 9c2960b16e..e3353bbf18 100644 --- a/experiments/bo_learner/yaml/babyA.yaml +++ b/experiments/bo_learner/yaml/babyA.yaml @@ -184,8 +184,8 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 1450 - n_cooldown_iterations: 1 + n_learning_iterations: 0 + n_cooldown_iterations: 0 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" diff --git a/experiments/roys_experiments/GridSearch.py b/experiments/roys_experiments/GridSearch.py new file mode 100644 index 0000000000..2b1c04832c --- /dev/null +++ b/experiments/roys_experiments/GridSearch.py @@ -0,0 +1,348 @@ +""" + experiments = [ + {'range_ub': 1.0, 'signal_factor_all': 1.0}, + {'range_ub': 1.0, 'signal_factor_all': 3.0}, + {'range_ub': 4.5, 'signal_factor_all': 3.0} + ] +""" +from sys import platform +import matplotlib +if platform == "darwin": + matplotlib.use("TkAgg") +import numpy as np +import itertools +import os +import time +import matplotlib.pyplot as plt +from glob import glob +from joblib import Parallel, delayed + +# Parameters +min_lines = 1490 +run_gazebo = False +n_runs = 20 # Naar 20 +n_jobs = 1 +my_yaml_path = "experiments/bo_learner/yaml/" +yaml_model = "gecko7.yaml" # CHANGETHIS ! +manager = "experiments/bo_learner/manager.py" +python_interpreter = "~/projects/revolve/.venv/bin/python3" + + + + +search_space = { + 'load_brain': ["'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.31482.txt'", + "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.33971.txt'", + "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.34137.txt'", + "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.44568.txt'"] + # 'evaluation_rate': [60], + # 'init_method': ["LHS"], + # 'verbose': [1], + # 'kernel_l': [0.1], + # 'acqui_ucb_alpha': [1.0], + # 'n_learning_iterations': [1450], + # 'n_init_samples': [50], + # 'kernel_l': [0.2], + # 'acqui_ucb_alpha': [3.0], + # 'n_learning_iterations': [950], + # 'n_init_samples': [20], +} + +print('search_space:', search_space) +# You don't have to change this +my_sub_directory = "yaml_temp/" +output_path = "output/cpg_bo/main_" + str(round(time.time())) + "/" +start_port = 12000 # STEP 6 +finished = False + +# Make in revolve/build to allow runs from terminal +# os.system('mkdir build9 && cd build9 && cmake ~/projects/revolve/ -DCMAKE_BUILD_TYPE="Release" -DLOCAL_GAZEBO_DIR=""') +# os.system("make -j4") + +def change_parameters(original_file, parameters): + # Iterate over dictionary elements + for key, value in iter(parameters.items()): + # Find first occurrence of parameter + ix = [row for row in range(len(original_file)) if key in original_file[row]][0] + + # Change values. Note the Python indentation + original_file[ix] = " " + key + " = " + str(value) + + # Return adapted file: + return original_file + + +def write_file(filename, contents): + # Write back to file + with open(filename, 'w') as file: + # Write updated contents + for line in contents: + file.write((line + "\n")) + file.close() + + +def create_yamls(yaml_path, model, sub_directory, experiments): + # Read original yaml file + yaml_file = [(line.rstrip('\n')) for line in open(yaml_path + model)] + + # Change yaml file + for my_dict in experiments: + # Loop over the entries in this dict + for key, value in my_dict.items(): + if not key == "id": + # Check on what line this parameter is + index = [ix for ix, x in enumerate(yaml_file) if key in x][0] + + yaml_file[index] = " " + key + ": " + str(value) + + # Write yaml file to desired location + write_file(yaml_path + sub_directory + "/" + yaml_model.split(".")[0] + "-" + str(my_dict["id"]) + ".yaml", yaml_file) + + +def run(i, sub_directory, model, params): + # Sleepy time when starting up to save gazebo from misery + if i < n_jobs: + time.sleep(5*i) + else: + print("Todo: Make sure you are leaving 2 seconds in between firing " + "gazebos") + + # Get yaml file id + k = params["id"] + + my_time = str(round(time.time(), 2)) + my_run_path = output_path + str(k) + "/" + my_time + "/" + os.mkdir(my_run_path) + + # Select relevant yaml file + yaml_model = my_yaml_path + sub_directory + model.split(".yaml")[0] + "-" + str(k) + ".yaml" + print(yaml_model) + # Get relevant yaml file + yaml_file = [(line.rstrip('\n')) for line in open(yaml_model)] + + # Change output_directory + index = [ix for ix, x in enumerate(yaml_file) if "output_directory" in x][0] + yaml_file[index] = f' output_directory: "{my_run_path}"' + + # Write temporarily with identifier + write_file(my_yaml_path + sub_directory + model.split(".yaml")[0] + "-" + str(k) + "-" + str(i) + ".yaml", yaml_file) + yaml_model = my_yaml_path + sub_directory + model.split(".yaml")[0] + "-" + str(k) + "-" + str(i) + ".yaml" + + # Change port: We need to do this via the manager + world_address = "127.0.0.1:" + str(start_port + i) + os.environ["GAZEBO_MASTER_URI"] = "http://localhost:" + str(start_port + i) + os.environ["LIBGL_ALWAYS_INDIRECT"] = "0" + py_command = "" + + # Call the experiment + if not run_gazebo: + py_command = python_interpreter + \ + " ./revolve.py" + \ + " --manager " + manager + \ + " --world-address " + world_address + \ + " --robot-yaml " + yaml_model + \ + " --port-start " + str(start_port + i) + else: + py_command = python_interpreter + \ + " ./revolve.py" + \ + " --manager " + manager + \ + " --world-address " + world_address + \ + " --robot-yaml " + yaml_model + \ + " --simulator-cmd gazebo" + \ + " --port-start " + str(start_port + i) + + return_code = os.system(py_command) + if return_code == 32512: + print("Specify a valid python interpreter in the parameters") + + +if __name__ == "__main__": + # Get permutations + keys, values = zip(*search_space.items()) + experiments = [dict(zip(keys, v)) for v in itertools.product(*values)] + + # PASTE THE EXPERIMENTS HERE, IN THE FORMAT SHOWN BELOW + # experiments = [ + # # {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0} # STEP 4 + # # + # # # # BASE RUN + # # # {'init_method': "LHS", 'kernel_l': 0.2, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 0.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 1.0, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 1.5, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.1, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.2, 'acqui_ucb_alpha': 0.5}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 0.5, 'acqui_ucb_alpha': 0.5}, + # # # # BASE RUN + # # # + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.1}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.2}, + # # # # BASE RUN + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.0}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 1.5}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 2.0}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 3.0}, + # # # {'init_method': "LHS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 4.0}, + # # # + # # # {'init_method': "RS", 'kernel_l': 0.1, 'kernel_sigma_sq': 1.0, 'acqui_ucb_alpha': 0.5}, + # # # BASE RUN + # # ] + # 'kernel_l': [0.02, 0.05, 0.1, 0.2], + # 'acqui_ucb_alpha': [0.1, 0.3, 0.5, 1.0], + unique_experiments = experiments + n_unique_experiments = len(experiments) + + # Get id's on the permutations + for ix, my_dict in enumerate(experiments): + my_dict["id"] = ix + experiments *=n_runs + + # Save to yaml files + create_yamls(yaml_path=my_yaml_path, + model=yaml_model, + sub_directory=my_sub_directory, + experiments=experiments + ) + + # Create dirs + if not os.path.isdir("output"): + os.mkdir("output") + if not os.path.isdir("output/cpg_bo"): + os.mkdir("output/cpg_bo") + os.mkdir(output_path) + + # Create experiment group directories + for i in range(n_unique_experiments): + os.mkdir(output_path + str(i) + "/") + + while not finished: + with Parallel(n_jobs=n_jobs) as parallel: + # Run experiments in parallel + try: + parallel(delayed(run)(i, + my_sub_directory, + yaml_model, + experiment) for i, experiment in enumerate(experiments)) + except: + print("Some runs are killed by timeout") + + # Count number of finished runs for all experiments. Read this from the parameters file + runs_succesful = {} + experiment_list = glob(output_path + "*/") + + for ix,e in enumerate(experiment_list): + runs = glob(e + "*/") + runs_succesful[str(e.split("/")[-2])] = 0 + + for my_run in runs: + if os.path.isfile(my_run + "fitnesses.txt"): + n_lines = len([(line.rstrip('\n')) for line in open(my_run + "fitnesses.txt")]) + + # In case we had a succesful run + if n_lines > min_lines: + runs_succesful[str(e.split("/")[-2])] += 1 + + to_run = {} + for key, val in runs_succesful.items(): + to_run[key] = n_runs - val + to_run = {k: v for k, v in to_run.items() if v > 0} + + # If the experiment list is empty + if not bool(to_run): + finished = True + else: + print(f"To finish {sum(to_run.values())} runs") + + # Empty experiments list + experiments = [] + + # Use spare computing capacity + while sum(to_run.values()) < n_jobs - len(to_run): + print(to_run) + + for key, value in to_run.items(): + to_run[key] += 1 + + # Construct new experiment list + for key, val in to_run.items(): + for i in range(val): + entry = unique_experiments[int(key)] + experiments.append(entry) + + # START ANALYSIS HERE + print("I will now perform analysis") + + # Do analysis + fitness_list = [] + for i in range(n_unique_experiments): + path = output_path + str(i) + "/*/" + path_list = glob(path) + path_list = [my_path for my_path in path_list if os.path.exists(my_path + "fitnesses.txt")] + n_rows = len([(line.rstrip('\n')) for line in open(path_list[0] + "fitnesses.txt")]) + n_subruns = len(path_list) + + # Working variables + fitnesses = np.empty((n_rows,n_subruns)) + fitnesses_mon = np.empty((n_rows,n_subruns)) + + # Create plot + plt.figure() + plt.title("Monotonic - Param setting " + str(i)) + plt.xlabel("Iteration") + plt.ylabel("Fitness") + plt.grid() + + # Get sub-runs for this setup + for ix, e in enumerate(path_list): + # Read fitness + my_fitness = [float((line.rstrip('\n'))) for line in open(e + "fitnesses.txt")] + + # Transfer fitness to monotonic sequence and save + my_fitness_mon = [e if e >= max(my_fitness[:ix+1]) else max(my_fitness[:ix+1]) for ix, e in enumerate(my_fitness)] + + # Save fitness + fitnesses_mon[:,ix] = np.array(my_fitness_mon) + fitnesses[:,ix] = np.array(my_fitness) + + # Plot the avg fitness + plt.plot(fitnesses_mon[:, ix], linewidth = 1, color = "blue") + + # Take average value over the n_runs + avg_fitness = np.mean(fitnesses, axis=1) + avg_fitness_mon = np.mean(fitnesses_mon, axis=1) + + # Save plot + plt.plot(avg_fitness_mon, linestyle="dashed", linewidth=2.5, color="black") + plt.tight_layout() + plt.savefig(output_path + str(i) + "/" + str(round(avg_fitness_mon[-1], 7)) + ".png") + + # Save fitness + fitness_list += [[round(avg_fitness_mon[-1], 5), i]] + + # Get fitness stats + fitness_list.sort(key=lambda x: x[0]) + fitness_list.reverse() + fitness_list + + print("Fitnesses are:") + for e in fitness_list: + print(e) + e = [str(e_) for e_ in e] + # Write to file in main directory + with open(output_path + '/results.txt', 'a') as avg_fitness_file: + avg_fitness_file.write(",".join(e) + "\n") + + print("Contents written to ", output_path) + + # Delete the yaml's + yaml_temp = my_yaml_path + my_sub_directory + yaml_files = glob(yaml_temp + "*") + for f in yaml_files: + os.remove(f) + + # Write the parameters + params_string =[] + for key, value in iter(search_space.items()): + params_string += [str(key) + ": " + str(value)] + write_file(output_path + "search_space.txt", params_string) diff --git a/cpprevolve/revolve/gazebo/battery/temp_charts.py b/experiments/roys_experiments/battery_chart.py similarity index 100% rename from cpprevolve/revolve/gazebo/battery/temp_charts.py rename to experiments/roys_experiments/battery_chart.py diff --git a/experiments/roys_experiments/charts.py b/experiments/roys_experiments/charts.py new file mode 100644 index 0000000000..f3c4e4707e --- /dev/null +++ b/experiments/roys_experiments/charts.py @@ -0,0 +1,148 @@ +import matplotlib +import matplotlib.pyplot as plt +import numpy as np +from os import listdir +from scipy.optimize import curve_fit +import pandas as pd + +from scipy.ndimage.filters import gaussian_filter1d + + +def draw_graph(title, xlabel, x, ylabel, y, avg = False): + plt.title(title) + plt.plot(x, y) + plt.ylabel(ylabel) + plt.xlabel(xlabel) + if avg: + plt.plot(x, gaussian_filter1d(y, sigma=3)) + plt.show() + + +def draw_both_gaussian(y1, y2, title = ''): + + plt.subplot(2, 1, 1) + if title == '': + plt.title('gaussian filter of average speed and energy used per iterations') + else: + plt.title(title) + plt.ylabel('energy') + plt.plot(range(1,121), y1, label='energy') + plt.plot(range(1,121), gaussian_filter1d(y1, sigma=3), label='energy') + + plt.subplot(2, 1, 2) + plt.ylabel('speed') + plt.xlabel('iteration') + plt.plot(range(1,121), y2, label='energy') + plt.plot(range(1,121), gaussian_filter1d(y2, sigma=3), label='speed') + plt.show() + + +def main(): + # matplotlib.use('MacOSX') + output_path = '../../../../output/cpg_bo/' + + robots = listdir(output_path) + robots.remove('.DS_Store') + + + all_robot_battery_avg = None + all_robot_speed_avg = None + + for robot in robots: + if '_' in robot and 'main' not in robot: + robot_battery_avg = None + robot_speed_avg = None + + time_info = listdir(output_path + robot) + + for time in time_info: + global_time = [] + watts_used = [] + current_charge = [] + temp_w = [] + temp_c = [] + temp_diff = [] + i = 1 + # VVV battery file VVV + with open(output_path + robot + '/' + time + '/battery.txt', 'r') as file: + for line in file: + data = line.strip('\n').split(' ') + global_time.append(float(data[0])) + watts_used.append(float(data[1])) + current_charge.append(float(data[2])) + + temp_w.append(float(data[1])) + temp_c.append(float(data[2])) + + if float(data[0]) > i * 60: + temp_diff.append(temp_c[0]-temp_c[-1]) + i += 1 + temp_c.clear() + temp_w.clear() + + # draw_graph('energy used per iteration for ' + robot + '\n iteration: ' + file_name, + # 'iteration', range(1, 121), 'total energy used', temp_diff) + + if robot_battery_avg is None: + robot_battery_avg = np.array(temp_diff) + else: + robot_battery_avg += np.array(temp_diff) + + # VVV speed file VVV + with open(output_path + robot + '/' + time + '/speed.txt', 'r') as file: + speed = [] + for line in file: + data = line.strip('\n') + speed.append(float(data)) + + # draw_graph('speed for ' + robot + '\n iteration: ' + num, 'iteration', range(1,121), 'speed', speed) + + if robot_speed_avg is None: + robot_speed_avg = np.array(speed) + else: + robot_speed_avg += np.array(speed) + robot_battery_avg /= 10 + # draw_graph('energy used per iteration for ' + robot + '\n average of all iterations ', + # 'iteration', range(1, 121), 'total energy used', robot_battery_avg) + + + robot_speed_avg /= 10 + # draw_graph('speed for ' + robot + '\n average of all iterations ', 'iteration', range(1,121), 'speed', robot_speed_avg) + + # draw_both_gaussian(robot_battery_avg, robot_speed_avg, 'Robot: ' + robot) + + if all_robot_battery_avg is None: + all_robot_battery_avg = np.array(robot_battery_avg) + else: + all_robot_battery_avg += np.array(robot_battery_avg) + + if all_robot_speed_avg is None: + all_robot_speed_avg = np.array(robot_speed_avg) + else: + all_robot_speed_avg += np.array(robot_speed_avg) + + all_robot_speed_avg /= 10 + all_robot_battery_avg /= 10 + + draw_graph('average speed for all robots per iteration', 'iterations', range(1, 121), 'average speed', all_robot_speed_avg, True) + + draw_graph('average energy used for all robots per iteration', 'iteration', range(1, 121), 'average energy used', all_robot_battery_avg, True) + + draw_both_gaussian(all_robot_battery_avg, all_robot_speed_avg) + + ratio = [] + for i in range(len(all_robot_speed_avg)): + ratio.append(all_robot_speed_avg[i]/all_robot_battery_avg[i]) + + draw_graph('ratio of average of all speed / average of all energy', 'iteration', range(1, 121), 'speed/energy', ratio, True) + + + + + + + + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/experiments/roys_experiments/manager.py b/experiments/roys_experiments/manager.py deleted file mode 100755 index f3213f5017..0000000000 --- a/experiments/roys_experiments/manager.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import asyncio - -# Add `..` folder in search path -current_dir = os.path.dirname(os.path.abspath(__file__)) -newpath = os.path.join(current_dir, '..', '..') -sys.path.append(newpath) - -from pygazebo.pygazebo import DisconnectError - -from pyrevolve import revolve_bot -from pyrevolve import parser -from pyrevolve.SDF.math import Vector3 -from pyrevolve.tol.manage import World -from pyrevolve.evolution import fitness -from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor - - -async def run(): - """ - The main coroutine, which is started below. - """ - # Parse command line / file input arguments - settings = parser.parse_args() - - # Load a robot from yaml - robot = revolve_bot.RevolveBot() - robot.load_file("experiments/bo_learner/yaml/spider9.yaml") - robot.update_substrate() - robot.save_file("./spider.sdf", conf_type='sdf') - - - # Start Simulator - if settings.simulator_cmd != 'debug': - simulator_supervisor = DynamicSimSupervisor( - world_file=settings.world, - simulator_cmd=settings.simulator_cmd, - simulator_args=["--verbose"], - plugins_dir_path=os.path.join('.', 'build', 'lib'), - models_dir_path=os.path.join('.', 'models'), - simulator_name='gazebo' - ) - await simulator_supervisor.launch_simulator(port=settings.port_start) - - # Connect to the simulator and pause - world = await World.create(settings) - await world.pause(True) - - await (await world.delete_model(robot.id)) - await asyncio.sleep(2.5) - - # Insert the robot in the simulator - insert_future = await world.insert_robot(robot, Vector3(0, 0, 0.25)) - robot_manager = await insert_future - - # Resume simulation - await world.pause(False) - - # Start a run loop to do some stuff - while True: - # Print robot fitness every second - # fitness_=fitness(robot_manager) - # print(f"Robot fitness is {fitness_}") - await asyncio.sleep(1.0) - - -def main(): - def handler(loop, context): - exc = context['exception'] - if isinstance(exc, DisconnectError) \ - or isinstance(exc, ConnectionResetError): - print("Got disconnect / connection reset - shutting down.") - sys.exit(0) - raise context['exception'] - - try: - loop = asyncio.get_event_loop() - loop.set_exception_handler(handler) - loop.run_until_complete(run()) - except KeyboardInterrupt: - print("Got CtrlC, shutting down.") - - -if __name__ == '__main__': - print("STARTING") - main() - print("FINISHED") diff --git a/cpprevolve/revolve/gazebo/battery/charts.py b/experiments/roys_experiments/temp_charts.py similarity index 52% rename from cpprevolve/revolve/gazebo/battery/charts.py rename to experiments/roys_experiments/temp_charts.py index 9c9199a06f..84707d1e9f 100644 --- a/cpprevolve/revolve/gazebo/battery/charts.py +++ b/experiments/roys_experiments/temp_charts.py @@ -45,9 +45,13 @@ def main(): battery_path = 'data/' output_path = '../../../../output/cpg_bo/' - for robot in robots: + all_robot_battery_avg = None + all_robot_speed_avg = None + for robot in robots: + robot_battery_avg = None + robot_speed_avg = None battery_info = [f for f in listdir(battery_path + robot) if f.endswith('.txt')] @@ -75,9 +79,18 @@ def main(): temp_c.clear() temp_w.clear() - df = pd.DataFrame({'global time':global_time, 'watts used':watts_used, 'current charge':current_charge}) - print(df.quantile([.1, .25, .5, .75], axis = 1)) - quit(0) + # draw_graph('energy used per iteration for ' + robot + '\n iteration: ' + file_name, + # 'iteration', range(1, 121), 'total energy used', temp_diff) + + if robot_battery_avg is None: + robot_battery_avg = np.array(temp_diff) + else: + robot_battery_avg += np.array(temp_diff) + + robot_battery_avg /= 10 + # draw_graph('energy used per iteration for ' + robot + '\n average of all iterations ', + # 'iteration', range(1, 121), 'total energy used', robot_battery_avg) + num_info = listdir(output_path + robot) @@ -89,6 +102,44 @@ def main(): data = line.strip('\n') speed.append(float(data)) + # draw_graph('speed for ' + robot + '\n iteration: ' + num, 'iteration', range(1,121), 'speed', speed) + + if robot_speed_avg is None: + robot_speed_avg = np.array(speed) + else: + robot_speed_avg += np.array(speed) + + robot_speed_avg /= 10 + # draw_graph('speed for ' + robot + '\n average of all iterations ', 'iteration', range(1,121), 'speed', robot_speed_avg) + + # draw_both_gaussian(robot_battery_avg, robot_speed_avg, 'Robot: ' + robot) + + if all_robot_battery_avg is None: + all_robot_battery_avg = np.array(robot_battery_avg) + else: + all_robot_battery_avg += np.array(robot_battery_avg) + + if all_robot_speed_avg is None: + all_robot_speed_avg = np.array(robot_speed_avg) + else: + all_robot_speed_avg += np.array(robot_speed_avg) + + all_robot_speed_avg /= 10 + all_robot_battery_avg /= 10 + + draw_graph('average speed for all robots per iteration', 'iterations', range(1, 121), 'average speed', all_robot_speed_avg, True) + + draw_graph('average energy used for all robots per iteration', 'iteration', range(1, 121), 'average energy used', all_robot_battery_avg, True) + + draw_both_gaussian(all_robot_battery_avg, all_robot_speed_avg) + + ratio = [] + for i in range(len(all_robot_speed_avg)): + ratio.append(all_robot_speed_avg[i]/all_robot_battery_avg[i]) + + draw_graph('ratio of average of all speed / average of all energy', 'iteration', range(1, 121), 'speed/energy', ratio, True) + + From 039c028c6fb1f13d08b60ec2789f5ce28d3a7262 Mon Sep 17 00:00:00 2001 From: roy-basmacier <42835471+roy-basmacier@users.noreply.github.com> Date: Thu, 29 Aug 2019 16:12:47 +0300 Subject: [PATCH 11/19] Update PositionMotor.cpp --- cpprevolve/revolve/gazebo/motors/PositionMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index 8f01d9a4b0..a296a732c3 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -130,7 +130,7 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) auto error = position - this->positionTarget_; - auto cmd = this->pid_.Update(error, stepTime); // angular velocity TODO this is targeted velocisty + auto cmd = this->pid_.Update(error, stepTime); // angular velocity TODO this is targeted velocity if (this->battery_) { From 577613959a38818954a3aed95aadf88d5607bb05 Mon Sep 17 00:00:00 2001 From: roy-basmacier <42835471+roy-basmacier@users.noreply.github.com> Date: Thu, 29 Aug 2019 16:15:13 +0300 Subject: [PATCH 12/19] Update PositionMotor.cpp --- cpprevolve/revolve/gazebo/motors/PositionMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index a296a732c3..314533f58f 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -139,8 +139,8 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) // TODO find which axis to use local or global // TODO check the watt if it should be positive or negative // TODO change this for now im using the absolute value of the watt so it always decreases from the joint movements - double watt = -abs(cmd * jointWrench.body1Torque.Length()); // TODO check which torque to use 1 or 2 - this->battery_->SetPowerLoad(this->consumerId_ , watt); + double power = -abs(cmd * jointWrench.body1Torque.Length()); // TODO check which torque to use 1 or 2 + this->battery_->SetPowerLoad(this->consumerId_ , power); } this->joint_->SetParam("vel", 0, cmd); From 04614a2fb963fcd83dc5592dfc5e978a14d5a898 Mon Sep 17 00:00:00 2001 From: roy-basmacier <42835471+roy-basmacier@users.noreply.github.com> Date: Thu, 29 Aug 2019 17:40:45 +0300 Subject: [PATCH 13/19] Update PositionMotor.cpp --- cpprevolve/revolve/gazebo/motors/PositionMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index 314533f58f..47d7594908 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -137,8 +137,8 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) ::gazebo::physics::JointWrench jointWrench = this->joint_->GetForceTorque(0); // TODO find which axis to use local or global - // TODO check the watt if it should be positive or negative - // TODO change this for now im using the absolute value of the watt so it always decreases from the joint movements + // TODO check the power if it should be positive or negative + // TODO change this for now im using the absolute value of the power so it always decreases from the joint movements double power = -abs(cmd * jointWrench.body1Torque.Length()); // TODO check which torque to use 1 or 2 this->battery_->SetPowerLoad(this->consumerId_ , power); From 7fdf4fa81697233941430996544f1fdb16a15499 Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Wed, 22 Apr 2020 02:53:54 -0700 Subject: [PATCH 14/19] Patch merge commit --- cpprevolve/revolve/gazebo/battery/Battery.cpp | 2 ++ .../revolve/gazebo/plugin/RobotController.cpp | 28 ++++++++++++++++++ .../revolve/gazebo/plugin/RobotController.h | 2 +- experiments/examples/manager.py | 29 +------------------ 4 files changed, 32 insertions(+), 29 deletions(-) diff --git a/cpprevolve/revolve/gazebo/battery/Battery.cpp b/cpprevolve/revolve/gazebo/battery/Battery.cpp index 09617d92e6..4432ca6021 100644 --- a/cpprevolve/revolve/gazebo/battery/Battery.cpp +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -21,9 +21,11 @@ void Battery::Update(double global_time, double delta_time) 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; + */ } diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 6d1884370e..799445eaab 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -112,6 +112,34 @@ void RobotController::Load( } } +///////////////////////////////////////////////// +void RobotController::UpdateBattery(ConstRequestPtr &_request) +{ + if (_request->data() not_eq this->model_->GetName() and + _request->data() not_eq this->model_->GetScopedName()) + { + return; + } + + gz::msgs::Response resp; + resp.set_id(_request->id()); + resp.set_request(_request->request()); + + if (_request->request() == "set_battery_level") + { + resp.set_response("success"); + this->SetBatteryLevel(_request->dbl_data()); + } + else + { + std::stringstream ss; + ss << this->BatteryLevel(); + resp.set_response(ss.str()); + } + + batterySetPub_->Publish(resp); +} + ///////////////////////////////////////////////// void RobotController::LoadActuators(const sdf::ElementPtr _sdf) { diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.h b/cpprevolve/revolve/gazebo/plugin/RobotController.h index 952294fb53..9c8258f6be 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.h +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.h @@ -145,7 +145,7 @@ namespace revolve protected: ::gazebo::physics::WorldPtr world_; /// \brief Shared pointer to the battery - protected: std::shared_ptr<::revolve::gazebo::Battery> battery_; + protected: std::shared_ptr battery_; /// \brief Driver update event pointer private: ::gazebo::event::ConnectionPtr updateConnection_; diff --git a/experiments/examples/manager.py b/experiments/examples/manager.py index 6aadb15a6e..27e06bb76e 100755 --- a/experiments/examples/manager.py +++ b/experiments/examples/manager.py @@ -9,14 +9,10 @@ from pyrevolve.SDF.math import Vector3 from pyrevolve import revolve_bot, parser from pyrevolve.tol.manage import World -<<<<<<< HEAD -from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor -from pyrevolve.evolution import fitness -======= from pyrevolve.evolution import fitness from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor ->>>>>>> 04614a2fb963fcd83dc5592dfc5e978a14d5a898 + async def run(): """ @@ -44,24 +40,6 @@ async def run(): robot = revolve_bot.RevolveBot() robot.load_file(robot_file_path) robot.update_substrate() -<<<<<<< HEAD - # robot._brain = BrainRLPowerSplines() -======= - robot.save_file("./spider.sdf", conf_type='sdf') - - - # Start Simulator - if settings.simulator_cmd != 'debug': - simulator_supervisor = DynamicSimSupervisor( - world_file=settings.world, - simulator_cmd=settings.simulator_cmd, - simulator_args=["--verbose"], - plugins_dir_path=os.path.join('.', 'build', 'lib'), - models_dir_path=os.path.join('.', 'models'), - simulator_name='gazebo' - ) - await simulator_supervisor.launch_simulator(port=settings.port_start) ->>>>>>> 04614a2fb963fcd83dc5592dfc5e978a14d5a898 # Connect to the simulator and pause connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) @@ -76,14 +54,9 @@ async def run(): # Start a run loop to do some stuff while True: # Print robot fitness every second -<<<<<<< HEAD status = 'dead' if robot_manager.dead else 'alive' print(f"Robot fitness ({status}) is \n" f" OLD: {fitness.online_old_revolve(robot_manager)}\n" f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n" f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}") -======= - fitness_=fitness.random(robot_manager) - print(f"Robot fitness is {fitness_}") ->>>>>>> 04614a2fb963fcd83dc5592dfc5e978a14d5a898 await asyncio.sleep(1.0) From a0dd716001f1673fed88dc77d384a9f32f64005d Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Thu, 23 Apr 2020 16:37:48 -0700 Subject: [PATCH 15/19] Integrating Battery with RobotController instead of WorldController. Adjusted proto message to include battery charge and managed endpoints in cpp and py to set and get battery value. Moved the robot state topic information about publishing frequency to the robot controller both in cpp and py to ensure that the robot is not blocked waiting on the world to respond on how much messages the robot has to send. Tried using the robot configuration to set the initial charge, but it is not accepting any value despite the changes in the sdf configuration and the way it is accessed in the cpp code. Furtermore the largest concern is the convoluted code surrounding the angle/.../world.py since it is closely connect to the way it gets the robot states, it is impossible to detach the responsibilities and move it to tol/.../world.py. This step requires refactoring of the entire robot/world paradigm. --- .../revolve/gazebo/msgs/robot_states.proto | 3 +- .../revolve/gazebo/plugin/RobotController.cpp | 187 +++++++++++++++++- .../revolve/gazebo/plugin/RobotController.h | 108 ++++++---- .../revolve/gazebo/plugin/WorldController.cpp | 174 +--------------- .../revolve/gazebo/plugin/WorldController.h | 18 -- experiments/examples/manager.py | 3 +- experiments/examples/yaml/spider.yaml | 3 + pyrevolve/angle/manage/robotmanager.py | 1 + pyrevolve/angle/manage/world.py | 7 +- pyrevolve/evolution/fitness.py | 4 + pyrevolve/spec/msgs/body_pb2.py | 34 ++-- pyrevolve/spec/msgs/model_inserted_pb2.py | 10 +- pyrevolve/spec/msgs/neural_net_pb2.py | 40 ++-- pyrevolve/spec/msgs/parameter_pb2.py | 8 +- pyrevolve/spec/msgs/robot_pb2.py | 12 +- pyrevolve/spec/msgs/robot_states_pb2.py | 39 ++-- pyrevolve/spec/msgs/sdf_body_analyze_pb2.py | 22 +-- pyrevolve/spec/msgs/spline_policy_pb2.py | 22 +-- pyrevolve/tol/manage/world.py | 7 + 19 files changed, 375 insertions(+), 327 deletions(-) diff --git a/cpprevolve/revolve/gazebo/msgs/robot_states.proto b/cpprevolve/revolve/gazebo/msgs/robot_states.proto index 60f77dce0f..e8d18ab615 100644 --- a/cpprevolve/revolve/gazebo/msgs/robot_states.proto +++ b/cpprevolve/revolve/gazebo/msgs/robot_states.proto @@ -7,7 +7,8 @@ message RobotState { required uint32 id = 1; required string name = 2; required gazebo.msgs.Pose pose = 3; - optional bool dead = 4; + optional double battery_charge = 4; + optional bool dead = 5; } message RobotStates { diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 799445eaab..8ea3dbbec4 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -37,9 +37,23 @@ using namespace revolve::gazebo; /// config in Load. RobotController::RobotController() : actuationTime_(0) + , robotStatesPubFreq_(5) + , lastRobotStatesUpdateTime_(0) { } +void unsubscribe(gz::transport::SubscriberPtr &subscription) +{ + if (subscription) + subscription->Unsubscribe(); +} + +void fini(gz::transport::PublisherPtr &publisher) +{ + if (publisher) + publisher->Fini(); +} + ///////////////////////////////////////////////// RobotController::~RobotController() { @@ -47,6 +61,12 @@ RobotController::~RobotController() this->world_.reset(); this->motorFactory_.reset(); this->sensorFactory_.reset(); + + unsubscribe(this->requestSub_); + + this->lastRobotStatesUpdateTime_ = 0; //this->world_->SimTime().Double(); + + fini(this->robotStatesPub_); } ///////////////////////////////////////////////// @@ -64,6 +84,16 @@ void RobotController::Load( this->node_.reset(new gz::transport::Node()); this->node_->Init(); + // Subscribe to insert request messages + this->requestSub_ = this->node_->Subscribe( + "~/request", + &RobotController::HandleRequest, + this); + + // Publisher for inserted models + this->responsePub_ = this->node_->Advertise< gz::msgs::Response >( + "~/response"); + // Subscribe to robot battery state updater this->batterySetSub_ = this->node_->Subscribe( "~/battery_level/request", @@ -72,6 +102,14 @@ void RobotController::Load( this->batterySetPub_ = this->node_->Advertise( "~/battery_level/response"); + // Bind to the world update event to perform some logic + this->onBeginUpdateConnection = gz::event::Events::ConnectWorldUpdateBegin( + [this] (const ::gazebo::common::UpdateInfo &_info) {this->OnBeginUpdate(_info);}); + + // Robot pose publisher + this->robotStatesPub_ = this->node_->Advertise< revolve::msgs::RobotStates >( + "~/revolve/robot_states", 500); + if (not _sdf->HasElement("rv:robot_config")) { std::cerr << "No `rv:robot_config` element found, controller not initialized." @@ -271,17 +309,11 @@ void RobotController::DoUpdate(const ::gazebo::common::UpdateInfo _info) // { // std::exit(0); // } - - auto currentTime = _info.simTime.Double() - initTime_; if (brain_) brain_->Update(motors_, sensors_, currentTime, actuationTime_); - /// exits out of simulation after 30 mins of simulation time -// if (initTime_ > 30) -// { -// std::exit(0); -// } + if(battery_) battery_->Update(currentTime, actuationTime_); } @@ -308,3 +340,144 @@ void RobotController::LoadBattery(const sdf::ElementPtr _sdf) } } + +///////////////////////////////////////////////// +void RobotController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) { + + if (not this->robotStatesPubFreq_) { + return; + } + + auto secs = 1.0 / this->robotStatesPubFreq_; + auto time = _info.simTime.Double(); + if ((time - this->lastRobotStatesUpdateTime_) >= secs) { + // Send robot info update message, this only sends the + // main pose of the robot (which is all we need for now) + msgs::RobotStates msg; + gz::msgs::Set(msg.mutable_time(), _info.simTime); + + { + boost::recursive_mutex::scoped_lock lock_physics(*this->world_->Physics()->GetPhysicsUpdateMutex()); + for (const auto &model : this->world_->Models()) { + if (model->IsStatic()) { + // Ignore static models such as the ground and obstacles + continue; + } + + revolve::msgs::RobotState *stateMsg = msg.add_robot_state(); + const std::string scoped_name = model->GetScopedName(); + stateMsg->set_name(scoped_name); + stateMsg->set_id(model->GetId()); + + auto poseMsg = stateMsg->mutable_pose(); + auto relativePose = model->RelativePose(); + + gz::msgs::Set(poseMsg, relativePose); + + // Death sentence check + const std::string name = model->GetName(); + bool death_sentence = false; + double death_sentence_value = 0; + { + boost::mutex::scoped_lock lock_death(death_sentences_mutex_); + death_sentence = death_sentences_.count(name) > 0; + if (death_sentence) + death_sentence_value = death_sentences_[name]; + } + + if (death_sentence) { + if (death_sentence_value < 0) { + // Initialize death sentence + death_sentences_[name] = time - death_sentence_value; + stateMsg->set_dead(false); + } else { + bool alive = death_sentence_value > time; + stateMsg->set_dead(not alive); + + if (not alive) { + boost::mutex::scoped_lock lock(this->death_sentences_mutex_); + this->death_sentences_.erase(model->GetName()); + + this->models_to_remove.emplace_back(model); + } + } + } + + if (this->battery_) { + stateMsg->set_battery_charge(this->battery_->current_charge); + } + } + } + + if (msg.robot_state_size() > 0) { + this->robotStatesPub_->Publish(msg); + this->lastRobotStatesUpdateTime_ = time; + } + } + + +// if (world_insert_remove_mutex.try_lock()) { + for (const auto &model: this->models_to_remove) { + std::cout << "Removing " << model->GetScopedName() << std::endl; +// this->world_->RemoveModel(model); +// gz::msgs::Request deleteReq; +// auto id = gz::physics::getUniqueId(); +// deleteReq.set_id(id); +// deleteReq.set_request("entity_delete"); +// deleteReq.set_data(model->GetScopedName()); +// this->requestPub_->Publish(deleteReq); + gz::transport::requestNoReply(this->world_->Name(), "entity_delete", model->GetScopedName()); + std::cout << "Removed " << model->GetScopedName() << std::endl; + + } + this->models_to_remove.clear(); +// this->world_insert_remove_mutex.unlock(); +// } +} + +///////////////////////////////////////////////// +// Process insert and delete requests +void RobotController::HandleRequest(ConstRequestPtr &request) { + std::cout << "RobotController handle request " << std::endl; + if (request->request() == "insert_sdf") + { + std::cout << "Processing insert model request ID `" << request->id() << "`." + << std::endl; + sdf::SDF robotSDF; + robotSDF.SetFromString(request->data()); + double lifespan_timeout = request->dbl_data(); + + // Get the model name, store in the expected map + auto name = robotSDF.Root()->GetElement("model")->GetAttribute("name") + ->GetAsString(); + + if (lifespan_timeout > 0) + { + boost::mutex::scoped_lock lock(death_sentences_mutex_); + // Initializes the death sentence negative because I don't dare to take the + // simulation time from this thread. + death_sentences_[name] = -lifespan_timeout; + } + //TODO insert here, it's better + //this->world_->InsertModelString(robotSDF.ToString()); + + // Don't leak memory + // https://bitbucket.org/osrf/sdformat/issues/104/memory-leak-in-element + robotSDF.Root()->Reset(); + } + else if (request->request() == "set_robot_state_update_frequency") + { + auto frequency = request->data(); + assert(frequency.find_first_not_of( "0123456789" ) == std::string::npos); + this->robotStatesPubFreq_ = (unsigned int)std::stoul(frequency); + std::cout << "Setting robot state update frequency to " + << this->robotStatesPubFreq_ << "." << std::endl; + + gz::msgs::Response resp; + resp.set_id(request->id()); + resp.set_request("set_robot_state_update_frequency"); + resp.set_response("success"); + + this->responsePub_->Publish(resp); + } +} \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.h b/cpprevolve/revolve/gazebo/plugin/RobotController.h index 9c8258f6be..bff3c8307e 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.h +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.h @@ -34,6 +34,9 @@ #include +#include +#include + namespace revolve { namespace gazebo @@ -41,114 +44,145 @@ namespace revolve class RobotController : public ::gazebo::ModelPlugin { + + public: /// \brief Constructor - public: RobotController(); + RobotController(); /// \brief Destructor - public: virtual ~RobotController(); + virtual ~RobotController(); /// \brief Load method - public: void Load( - ::gazebo::physics::ModelPtr _parent, - sdf::ElementPtr _sdf) override; + void Load(::gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf) override; /// \return Factory class that creates motors for this model - public: virtual MotorFactoryPtr MotorFactory( - ::gazebo::physics::ModelPtr _model); + virtual MotorFactoryPtr MotorFactory(::gazebo::physics::ModelPtr _model); /// \return Factory class that creates motors for this robot - public: virtual SensorFactoryPtr SensorFactory( - ::gazebo::physics::ModelPtr _model); + virtual SensorFactoryPtr SensorFactory(::gazebo::physics::ModelPtr _model); /// \brief Update event which, by default, is called periodically /// according to the update rate specified in the robot plugin. - public: virtual void DoUpdate(const ::gazebo::common::UpdateInfo _info); + virtual void DoUpdate(const ::gazebo::common::UpdateInfo _info); /// \brief Returns the battery level /// \details Methods allows reading and writing the battery level in /// the robot SDF. This is mostly useful for the `BatterySensor` to /// obtain the battery state, and storing it in the SDF also means it /// will be adequately backed up in an eventual snapshot. - public: double BatteryLevel(); + double BatteryLevel(); /// \brief Sets the battery level if possible - public: void SetBatteryLevel(double _level); + void SetBatteryLevel(double _level); /// \brief Request listener for battery update - public: void UpdateBattery(ConstRequestPtr &_request); + void UpdateBattery(ConstRequestPtr &_request); + protected: /// \brief Detects and loads motors in the plugin spec - protected: virtual void LoadActuators(const sdf::ElementPtr _sdf); + virtual void LoadActuators(const sdf::ElementPtr _sdf); /// \brief Detects and loads sensors in the plugin spec. - protected: virtual void LoadSensors(const sdf::ElementPtr _sdf); + virtual void LoadSensors(const sdf::ElementPtr _sdf); /// \brief Loads the brain from the `rv:brain` element. /// \details By default this tries to construct a `StandardNeuralNetwork`. - protected: virtual void LoadBrain(const sdf::ElementPtr _sdf); + virtual void LoadBrain(const sdf::ElementPtr _sdf); /// \brief Loads / initializes the robot battery - protected: virtual void LoadBattery(const sdf::ElementPtr _sdf); + virtual void LoadBattery(const sdf::ElementPtr _sdf); + + // Method called + virtual void OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info); /// \brief Method called at the end of the default `Load` function. /// \details This should be used to initialize robot actuation, i.e. /// register some update event. By default, this grabs the /// `update_rate` from the robot config pointer, and binds - protected: virtual void Startup( - ::gazebo::physics::ModelPtr _parent, - sdf::ElementPtr _sdf); + virtual void Startup(::gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf); /// \brief Default method bound to world update event, checks whether the /// \brief actuation time has passed and updates if required. - protected: void CheckUpdate(const ::gazebo::common::UpdateInfo _info); + void CheckUpdate(const ::gazebo::common::UpdateInfo _info); + + // Listener for analysis requests + virtual void HandleRequest(ConstRequestPtr &request); /// \brief Networking node - protected: ::gazebo::transport::NodePtr node_; + ::gazebo::transport::NodePtr node_; /// \brief Subscriber for battery update request - protected: ::gazebo::transport::SubscriberPtr batterySetSub_; + ::gazebo::transport::SubscriberPtr batterySetSub_; /// \brief Responder for battery update request - protected: ::gazebo::transport::PublisherPtr batterySetPub_; + ::gazebo::transport::PublisherPtr batterySetPub_; + + ::gazebo::event::ConnectionPtr onBeginUpdateConnection; + + // Response publisher + ::gazebo::transport::PublisherPtr responsePub_; /// \brief Holds an instance of the motor factory - protected: MotorFactoryPtr motorFactory_; + MotorFactoryPtr motorFactory_; /// \brief Holds an instance of the sensor factory - protected: SensorFactoryPtr sensorFactory_; + SensorFactoryPtr sensorFactory_; /// \brief Brain controlling this model - protected: BrainPtr brain_; + BrainPtr brain_; /// \brief Actuation time, in seconds - protected: double actuationTime_; + double actuationTime_; /// \brief Time of initialisation - protected: double initTime_; + double initTime_; /// \brief rv:battery element, if present - protected: sdf::ElementPtr batteryElem_; + sdf::ElementPtr batteryElem_; /// \brief Time of the last actuation, in seconds and nanoseconds - protected: ::gazebo::common::Time lastActuationTime_; + ::gazebo::common::Time lastActuationTime_; /// \brief Motors in this model - protected: std::vector< MotorPtr > motors_; + std::vector< MotorPtr > motors_; /// \brief Sensors in this model - protected: std::vector< SensorPtr > sensors_; + std::vector< SensorPtr > sensors_; /// \brief Pointer to the model - protected: ::gazebo::physics::ModelPtr model_; + ::gazebo::physics::ModelPtr model_; /// \brief Pointer to the world - protected: ::gazebo::physics::WorldPtr world_; + ::gazebo::physics::WorldPtr world_; /// \brief Shared pointer to the battery - protected: std::shared_ptr battery_; + std::shared_ptr battery_; + + ::gazebo::physics::Model_V models_to_remove; + + // Request subscriber + ::gazebo::transport::SubscriberPtr requestSub_; + + // Death sentence list. It collects all the end time for all robots that have + // a death sentence + // NEGATIVE DEATH SENTENCES mean total lifetime, death sentence not yet initialized. + std::map death_sentences_; + + // Mutex for the deleteMap_ + boost::mutex death_sentences_mutex_; + + // Publisher for periodic robot poses + ::gazebo::transport::PublisherPtr robotStatesPub_; + + // Frequency at which robot info is published + // Defaults to 0, which means no update at all + unsigned int robotStatesPubFreq_; + // Last (simulation) time robot info was sent + double lastRobotStatesUpdateTime_; + private: /// \brief Driver update event pointer - private: ::gazebo::event::ConnectionPtr updateConnection_; + ::gazebo::event::ConnectionPtr updateConnection_; }; } /* namespace gazebo */ } /* namespace revolve */ diff --git a/cpprevolve/revolve/gazebo/plugin/WorldController.cpp b/cpprevolve/revolve/gazebo/plugin/WorldController.cpp index ea491681d7..4da3555c17 100644 --- a/cpprevolve/revolve/gazebo/plugin/WorldController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/WorldController.cpp @@ -28,8 +28,6 @@ using namespace revolve::gazebo; ///////////////////////////////////////////////// WorldController::WorldController() : delete_robot_queue() - , robotStatesPubFreq_(5) - , lastRobotStatesUpdateTime_(0) { } @@ -119,96 +117,11 @@ void WorldController::Load( void WorldController::Reset() { - this->lastRobotStatesUpdateTime_ = 0; //this->world_->SimTime().Double(); } ///////////////////////////////////////////////// void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) { - if (not this->robotStatesPubFreq_) { - return; - } - - auto secs = 1.0 / this->robotStatesPubFreq_; - auto time = _info.simTime.Double(); - if ((time - this->lastRobotStatesUpdateTime_) >= secs) { - // Send robot info update message, this only sends the - // main pose of the robot (which is all we need for now) - msgs::RobotStates msg; - gz::msgs::Set(msg.mutable_time(), _info.simTime); - - { - boost::recursive_mutex::scoped_lock lock_physics(*this->world_->Physics()->GetPhysicsUpdateMutex()); - for (const auto &model : this->world_->Models()) { - if (model->IsStatic()) { - // Ignore static models such as the ground and obstacles - continue; - } - - revolve::msgs::RobotState *stateMsg = msg.add_robot_state(); - const std::string scoped_name = model->GetScopedName(); - stateMsg->set_name(scoped_name); - stateMsg->set_id(model->GetId()); - - auto poseMsg = stateMsg->mutable_pose(); - auto relativePose = model->RelativePose(); - - gz::msgs::Set(poseMsg, relativePose); - - // Death sentence check - const std::string name = model->GetName(); - bool death_sentence = false; - double death_sentence_value = 0; - { - boost::mutex::scoped_lock lock_death(death_sentences_mutex_); - death_sentence = death_sentences_.count(name) > 0; - if (death_sentence) - death_sentence_value = death_sentences_[name]; - } - - if (death_sentence) { - if (death_sentence_value < 0) { - // Initialize death sentence - death_sentences_[name] = time - death_sentence_value; - stateMsg->set_dead(false); - } else { - bool alive = death_sentence_value > time; - stateMsg->set_dead(not alive); - - if (not alive) { - boost::mutex::scoped_lock lock(this->death_sentences_mutex_); - this->death_sentences_.erase(model->GetName()); - - this->models_to_remove.emplace_back(model); - } - } - } - } - } - - if (msg.robot_state_size() > 0) { - this->robotStatesPub_->Publish(msg); - this->lastRobotStatesUpdateTime_ = time; - } - } - - -// if (world_insert_remove_mutex.try_lock()) { - for (const auto &model: this->models_to_remove) { - std::cout << "Removing " << model->GetScopedName() << std::endl; -// this->world_->RemoveModel(model); -// gz::msgs::Request deleteReq; -// auto id = gz::physics::getUniqueId(); -// deleteReq.set_id(id); -// deleteReq.set_request("entity_delete"); -// deleteReq.set_data(model->GetScopedName()); -// this->requestPub_->Publish(deleteReq); - gz::transport::requestNoReply(this->world_->Name(), "entity_delete", model->GetScopedName()); - std::cout << "Removed " << model->GetScopedName() << std::endl; - } - this->models_to_remove.clear(); -// this->world_insert_remove_mutex.unlock(); -// } } void WorldController::OnEndUpdate() @@ -270,44 +183,13 @@ void WorldController::HandleRequest(ConstRequestPtr &request) std::cout << "Processing request `" << request->id() << "` to delete robot `" << name << "`" << std::endl; -// auto model = this->world_->ModelByName(name); -// if (model) -// { -// // Tell the world to remove the model -// // Using `World::RemoveModel()` from here crashes the transport -// // library, the cause of which I've yet to figure out - it has -// // something to do with race conditions where the model is used by -// // the world while it is being updated. Fixing this completely -// // appears to be a rather involved process, instead, we'll use an -// // `entity_delete` request, which will make sure deleting the model -// // happens on the world thread. -// gz::msgs::Request deleteReq; -// auto id = gz::physics::getUniqueId(); -// deleteReq.set_id(id); -// deleteReq.set_request("entity_delete"); -// deleteReq.set_data(model->GetScopedName()); -// -// { -// boost::mutex::scoped_lock lock(this->deleteMutex_); -// this->delete_robot_queue.emplace(std::make_tuple(model, request->id())); -// } -// { -// boost::mutex::scoped_lock lock(this->death_sentences_mutex_); -// this->death_sentences_.erase(model->GetName()); -// } -// -// this->requestPub_->Publish(deleteReq); -// } -// else -// { - std::cerr << "Model `" << name << "` could not be found in the world." - << std::endl; - gz::msgs::Response resp; - resp.set_id(request->id()); - resp.set_request("delete_robot"); - resp.set_response("error"); - this->responsePub_->Publish(resp); -// } + std::cerr << "Model `" << name << "` could not be found in the world." + << std::endl; + gz::msgs::Response resp; + resp.set_id(request->id()); + resp.set_request("delete_robot"); + resp.set_response("error"); + this->responsePub_->Publish(resp); } else if (request->request() == "insert_sdf") { @@ -321,14 +203,6 @@ void WorldController::HandleRequest(ConstRequestPtr &request) auto name = robotSDF.Root()->GetElement("model")->GetAttribute("name") ->GetAsString(); - if (lifespan_timeout > 0) - { - boost::mutex::scoped_lock lock(death_sentences_mutex_); - // Initializes the death sentence negative because I don't dare to take the - // simulation time from this thread. - death_sentences_[name] = -lifespan_timeout; - } - { boost::mutex::scoped_lock lock(this->insertMutex_); this->insertMap_[name] = std::make_tuple(request->id(), robotSDF.ToString(), true); @@ -341,21 +215,6 @@ void WorldController::HandleRequest(ConstRequestPtr &request) // https://bitbucket.org/osrf/sdformat/issues/104/memory-leak-in-element robotSDF.Root()->Reset(); } - else if (request->request() == "set_robot_state_update_frequency") - { - auto frequency = request->data(); - assert(frequency.find_first_not_of( "0123456789" ) == std::string::npos); - this->robotStatesPubFreq_ = (unsigned int)std::stoul(frequency); - std::cout << "Setting robot state update frequency to " - << this->robotStatesPubFreq_ << "." << std::endl; - - gz::msgs::Response resp; - resp.set_id(request->id()); - resp.set_request("set_robot_state_update_frequency"); - resp.set_response("success"); - - this->responsePub_->Publish(resp); - } } ///////////////////////////////////////////////// @@ -415,23 +274,4 @@ void WorldController::HandleResponse(ConstResponsePtr &response) return; } -// int id; -// { -// boost::mutex::scoped_lock lock(this->deleteMutex_); -// if (this->deleteMap_.count(response->id()) <= 0) -// { -// return; -// } -// -// id = this->deleteMap_[response->id()]; -// this->deleteMap_.erase(id); -// } - -// this->world_insert_remove_mutex.unlock(); - -// gz::msgs::Response resp; -// resp.set_id(id); -// resp.set_request("delete_robot"); -// resp.set_response("success"); -// this->responsePub_->Publish(resp); } diff --git a/cpprevolve/revolve/gazebo/plugin/WorldController.h b/cpprevolve/revolve/gazebo/plugin/WorldController.h index 359badbd96..ac892988a5 100644 --- a/cpprevolve/revolve/gazebo/plugin/WorldController.h +++ b/cpprevolve/revolve/gazebo/plugin/WorldController.h @@ -106,28 +106,10 @@ class WorldController: public ::gazebo::WorldPlugin // Publisher for periodic robot poses ::gazebo::transport::PublisherPtr robotStatesPub_; - // Frequency at which robot info is published - // Defaults to 0, which means no update at all - unsigned int robotStatesPubFreq_; - // Pointer to the update event connection ::gazebo::event::ConnectionPtr onBeginUpdateConnection; ::gazebo::event::ConnectionPtr onEndUpdateConnection; - // Last (simulation) time robot info was sent - double lastRobotStatesUpdateTime_; - - // Death sentence list. It collects all the end time for all robots that have - // a death sentence - // NEGATIVE DEATH SENTENCES mean total lifetime, death sentence not yet initialized. - std::map death_sentences_; - - // Mutex for the deleteMap_ - boost::mutex death_sentences_mutex_; - -// boost::mutex world_insert_remove_mutex; - - ::gazebo::physics::Model_V models_to_remove; }; } // namespace gazebo diff --git a/experiments/examples/manager.py b/experiments/examples/manager.py index 27e06bb76e..18afd516ce 100755 --- a/experiments/examples/manager.py +++ b/experiments/examples/manager.py @@ -58,5 +58,6 @@ async def run(): print(f"Robot fitness ({status}) is \n" f" OLD: {fitness.online_old_revolve(robot_manager)}\n" f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n" - f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}") + f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}\n" + f" battery: {fitness.battery(robot_manager, robot)}") await asyncio.sleep(1.0) diff --git a/experiments/examples/yaml/spider.yaml b/experiments/examples/yaml/spider.yaml index 5ca194386b..0e80697d1a 100644 --- a/experiments/examples/yaml/spider.yaml +++ b/experiments/examples/yaml/spider.yaml @@ -159,3 +159,6 @@ brain: abs_output_bound : 1.0; weights: [0.482167, 0.560357, 0.753772, 0.221536, 0.44513, 0.667353, 0.580933, 0.246228, 0.111797, 0.110425, 0.667353, 0.519204, 0.11134, 0.667353, 0.70439, 0.000228624, 0.444673, 0.287837] +robot_config: + battery: + initial_charge: 1.0 \ No newline at end of file diff --git a/pyrevolve/angle/manage/robotmanager.py b/pyrevolve/angle/manage/robotmanager.py index 678fa50bfc..94531310d3 100644 --- a/pyrevolve/angle/manage/robotmanager.py +++ b/pyrevolve/angle/manage/robotmanager.py @@ -87,6 +87,7 @@ def update_state(self, world, time, state, poses_file): """ dead = state.dead if state.dead is not None else False self.dead = dead or self.dead + self.battery_level = state.battery_charge pos = state.pose.position position = Vector3(pos.x, pos.y, pos.z) diff --git a/pyrevolve/angle/manage/world.py b/pyrevolve/angle/manage/world.py index 31c8fd9a9b..f4d032b34f 100644 --- a/pyrevolve/angle/manage/world.py +++ b/pyrevolve/angle/manage/world.py @@ -214,11 +214,6 @@ async def _init(self): self._update_contacts ) - # Awaiting this immediately will lock the program - update_state_future = self.set_state_update_frequency( - freq=self.state_update_frequency - ) - self.battery_handler = await RequestHandler.create( manager=self.manager, advertise='/gazebo/default/battery_level/request', @@ -231,7 +226,6 @@ async def _init(self): # Wait for connections await self.pose_subscriber.wait_for_connection() await self.contact_subscriber.wait_for_connection() - await update_state_future if self.do_restore: await (self.restore_snapshot(self.do_restore)) @@ -453,6 +447,7 @@ async def insert_robot( robot=revolve_bot, msg=response ) + return robot_manager def to_sdfbot( diff --git a/pyrevolve/evolution/fitness.py b/pyrevolve/evolution/fitness.py index 95be1c03cf..f766d90139 100644 --- a/pyrevolve/evolution/fitness.py +++ b/pyrevolve/evolution/fitness.py @@ -20,6 +20,10 @@ def displacement_velocity(robot_manager, robot): return measures.displacement_velocity(robot_manager) +def battery(robot_manager, robot): + return robot_manager.battery_level + + def online_old_revolve(robot_manager): """ Fitness is proportional to both the displacement and absolute diff --git a/pyrevolve/spec/msgs/body_pb2.py b/pyrevolve/spec/msgs/body_pb2.py index b6aa3ef246..da2dde3dae 100644 --- a/pyrevolve/spec/msgs/body_pb2.py +++ b/pyrevolve/spec/msgs/body_pb2.py @@ -7,6 +7,7 @@ from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() @@ -19,10 +20,10 @@ name='body.proto', package='revolve.msgs', syntax='proto2', - serialized_options=None, serialized_pb=_b('\n\nbody.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"\xb3\x01\n\x08\x42odyPart\x12\n\n\x02id\x18\x01 \x02(\t\x12\x0c\n\x04type\x18\x02 \x02(\t\x12\t\n\x01x\x18\x03 \x02(\x05\x12\t\n\x01y\x18\x04 \x02(\x05\x12\x13\n\x0borientation\x18\x05 \x02(\x01\x12+\n\x05\x63hild\x18\x06 \x03(\x0b\x32\x1c.revolve.msgs.BodyConnection\x12&\n\x05param\x18\x07 \x03(\x0b\x32\x17.revolve.msgs.Parameter\x12\r\n\x05label\x18\x08 \x01(\t\"Z\n\x0e\x42odyConnection\x12\x10\n\x08src_slot\x18\x01 \x02(\x05\x12\x10\n\x08\x64st_slot\x18\x02 \x02(\x05\x12$\n\x04part\x18\x03 \x02(\x0b\x32\x16.revolve.msgs.BodyPart\",\n\x04\x42ody\x12$\n\x04root\x18\x01 \x02(\x0b\x32\x16.revolve.msgs.BodyPart') , dependencies=[parameter__pb2.DESCRIPTOR,]) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) @@ -40,63 +41,63 @@ has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='type', full_name='revolve.msgs.BodyPart.type', index=1, number=2, type=9, cpp_type=9, label=2, has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='x', full_name='revolve.msgs.BodyPart.x', index=2, number=3, type=5, cpp_type=1, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='y', full_name='revolve.msgs.BodyPart.y', index=3, number=4, type=5, cpp_type=1, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='orientation', full_name='revolve.msgs.BodyPart.orientation', index=4, number=5, type=1, cpp_type=5, label=2, has_default_value=False, default_value=float(0), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='child', full_name='revolve.msgs.BodyPart.child', index=5, number=6, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='param', full_name='revolve.msgs.BodyPart.param', index=6, number=7, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='label', full_name='revolve.msgs.BodyPart.label', index=7, number=8, type=9, cpp_type=9, label=1, has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -120,28 +121,28 @@ has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='dst_slot', full_name='revolve.msgs.BodyConnection.dst_slot', index=1, number=2, type=5, cpp_type=1, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='part', full_name='revolve.msgs.BodyConnection.part', index=2, number=3, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -165,14 +166,14 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -189,7 +190,6 @@ DESCRIPTOR.message_types_by_name['BodyPart'] = _BODYPART DESCRIPTOR.message_types_by_name['BodyConnection'] = _BODYCONNECTION DESCRIPTOR.message_types_by_name['Body'] = _BODY -_sym_db.RegisterFileDescriptor(DESCRIPTOR) BodyPart = _reflection.GeneratedProtocolMessageType('BodyPart', (_message.Message,), dict( DESCRIPTOR = _BODYPART, diff --git a/pyrevolve/spec/msgs/model_inserted_pb2.py b/pyrevolve/spec/msgs/model_inserted_pb2.py index 3e1056bcd0..751f0d71a2 100644 --- a/pyrevolve/spec/msgs/model_inserted_pb2.py +++ b/pyrevolve/spec/msgs/model_inserted_pb2.py @@ -7,6 +7,7 @@ from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() @@ -20,10 +21,10 @@ name='model_inserted.proto', package='revolve.msgs', syntax='proto2', - serialized_options=None, serialized_pb=_b('\n\x14model_inserted.proto\x12\x0crevolve.msgs\x1a\x0bmodel.proto\x1a\ntime.proto\"S\n\rModelInserted\x12\x1f\n\x04time\x18\x01 \x02(\x0b\x32\x11.gazebo.msgs.Time\x12!\n\x05model\x18\x02 \x02(\x0b\x32\x12.gazebo.msgs.Model') , dependencies=[model__pb2.DESCRIPTOR,time__pb2.DESCRIPTOR,]) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) @@ -41,21 +42,21 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='model', full_name='revolve.msgs.ModelInserted.model', index=1, number=2, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -68,7 +69,6 @@ _MODELINSERTED.fields_by_name['time'].message_type = time__pb2._TIME _MODELINSERTED.fields_by_name['model'].message_type = model__pb2._MODEL DESCRIPTOR.message_types_by_name['ModelInserted'] = _MODELINSERTED -_sym_db.RegisterFileDescriptor(DESCRIPTOR) ModelInserted = _reflection.GeneratedProtocolMessageType('ModelInserted', (_message.Message,), dict( DESCRIPTOR = _MODELINSERTED, diff --git a/pyrevolve/spec/msgs/neural_net_pb2.py b/pyrevolve/spec/msgs/neural_net_pb2.py index 7f9d29f97b..af38257473 100644 --- a/pyrevolve/spec/msgs/neural_net_pb2.py +++ b/pyrevolve/spec/msgs/neural_net_pb2.py @@ -7,6 +7,7 @@ from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() @@ -19,10 +20,10 @@ name='neural_net.proto', package='revolve.msgs', syntax='proto2', - serialized_options=None, serialized_pb=_b('\n\x10neural_net.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"<\n\x10NeuralConnection\x12\x0b\n\x03src\x18\x01 \x02(\t\x12\x0b\n\x03\x64st\x18\x02 \x02(\t\x12\x0e\n\x06weight\x18\x03 \x02(\x01\"i\n\x06Neuron\x12\n\n\x02id\x18\x01 \x02(\t\x12\r\n\x05layer\x18\x02 \x02(\t\x12\x0c\n\x04type\x18\x03 \x02(\t\x12\x0e\n\x06partId\x18\x04 \x01(\t\x12&\n\x05param\x18\x05 \x03(\x0b\x32\x17.revolve.msgs.Parameter\"i\n\rNeuralNetwork\x12$\n\x06neuron\x18\x01 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12\x32\n\nconnection\x18\x02 \x03(\x0b\x32\x1e.revolve.msgs.NeuralConnection\"\xb9\x01\n\x13ModifyNeuralNetwork\x12\x15\n\rremove_hidden\x18\x01 \x03(\t\x12(\n\nadd_hidden\x18\x02 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12,\n\x0eset_parameters\x18\x04 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12\x33\n\x0bset_weights\x18\x03 \x03(\x0b\x32\x1e.revolve.msgs.NeuralConnection') , dependencies=[parameter__pb2.DESCRIPTOR,]) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) @@ -40,28 +41,28 @@ has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='dst', full_name='revolve.msgs.NeuralConnection.dst', index=1, number=2, type=9, cpp_type=9, label=2, has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='weight', full_name='revolve.msgs.NeuralConnection.weight', index=2, number=3, type=1, cpp_type=5, label=2, has_default_value=False, default_value=float(0), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -85,42 +86,42 @@ has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='layer', full_name='revolve.msgs.Neuron.layer', index=1, number=2, type=9, cpp_type=9, label=2, has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='type', full_name='revolve.msgs.Neuron.type', index=2, number=3, type=9, cpp_type=9, label=2, has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='partId', full_name='revolve.msgs.Neuron.partId', index=3, number=4, type=9, cpp_type=9, label=1, has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='param', full_name='revolve.msgs.Neuron.param', index=4, number=5, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -144,21 +145,21 @@ has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='connection', full_name='revolve.msgs.NeuralNetwork.connection', index=1, number=2, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -182,35 +183,35 @@ has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='add_hidden', full_name='revolve.msgs.ModifyNeuralNetwork.add_hidden', index=1, number=2, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='set_parameters', full_name='revolve.msgs.ModifyNeuralNetwork.set_parameters', index=2, number=4, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='set_weights', full_name='revolve.msgs.ModifyNeuralNetwork.set_weights', index=3, number=3, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -230,7 +231,6 @@ DESCRIPTOR.message_types_by_name['Neuron'] = _NEURON DESCRIPTOR.message_types_by_name['NeuralNetwork'] = _NEURALNETWORK DESCRIPTOR.message_types_by_name['ModifyNeuralNetwork'] = _MODIFYNEURALNETWORK -_sym_db.RegisterFileDescriptor(DESCRIPTOR) NeuralConnection = _reflection.GeneratedProtocolMessageType('NeuralConnection', (_message.Message,), dict( DESCRIPTOR = _NEURALCONNECTION, diff --git a/pyrevolve/spec/msgs/parameter_pb2.py b/pyrevolve/spec/msgs/parameter_pb2.py index c666d55583..1d57a91fe8 100644 --- a/pyrevolve/spec/msgs/parameter_pb2.py +++ b/pyrevolve/spec/msgs/parameter_pb2.py @@ -7,6 +7,7 @@ from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() @@ -18,9 +19,9 @@ name='parameter.proto', package='revolve.msgs', syntax='proto2', - serialized_options=None, serialized_pb=_b('\n\x0fparameter.proto\x12\x0crevolve.msgs\"\x1a\n\tParameter\x12\r\n\x05value\x18\x01 \x02(\x01') ) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) @@ -38,14 +39,14 @@ has_default_value=False, default_value=float(0), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -56,7 +57,6 @@ ) DESCRIPTOR.message_types_by_name['Parameter'] = _PARAMETER -_sym_db.RegisterFileDescriptor(DESCRIPTOR) Parameter = _reflection.GeneratedProtocolMessageType('Parameter', (_message.Message,), dict( DESCRIPTOR = _PARAMETER, diff --git a/pyrevolve/spec/msgs/robot_pb2.py b/pyrevolve/spec/msgs/robot_pb2.py index 885f0a1630..507985eca1 100644 --- a/pyrevolve/spec/msgs/robot_pb2.py +++ b/pyrevolve/spec/msgs/robot_pb2.py @@ -7,6 +7,7 @@ from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() @@ -20,10 +21,10 @@ name='robot.proto', package='revolve.msgs', syntax='proto2', - serialized_options=None, serialized_pb=_b('\n\x0brobot.proto\x12\x0crevolve.msgs\x1a\nbody.proto\x1a\x10neural_net.proto\"a\n\x05Robot\x12\n\n\x02id\x18\x01 \x02(\x05\x12 \n\x04\x62ody\x18\x02 \x02(\x0b\x32\x12.revolve.msgs.Body\x12*\n\x05\x62rain\x18\x03 \x02(\x0b\x32\x1b.revolve.msgs.NeuralNetwork') , dependencies=[body__pb2.DESCRIPTOR,neural__net__pb2.DESCRIPTOR,]) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) @@ -41,28 +42,28 @@ has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='body', full_name='revolve.msgs.Robot.body', index=1, number=2, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='brain', full_name='revolve.msgs.Robot.brain', index=2, number=3, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -75,7 +76,6 @@ _ROBOT.fields_by_name['body'].message_type = body__pb2._BODY _ROBOT.fields_by_name['brain'].message_type = neural__net__pb2._NEURALNETWORK DESCRIPTOR.message_types_by_name['Robot'] = _ROBOT -_sym_db.RegisterFileDescriptor(DESCRIPTOR) Robot = _reflection.GeneratedProtocolMessageType('Robot', (_message.Message,), dict( DESCRIPTOR = _ROBOT, diff --git a/pyrevolve/spec/msgs/robot_states_pb2.py b/pyrevolve/spec/msgs/robot_states_pb2.py index 9d09a2680d..ab5b88e2da 100644 --- a/pyrevolve/spec/msgs/robot_states_pb2.py +++ b/pyrevolve/spec/msgs/robot_states_pb2.py @@ -7,6 +7,7 @@ from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() @@ -20,10 +21,10 @@ name='robot_states.proto', package='revolve.msgs', syntax='proto2', - serialized_options=None, - serialized_pb=_b('\n\x12robot_states.proto\x12\x0crevolve.msgs\x1a\ntime.proto\x1a\npose.proto\"U\n\nRobotState\x12\n\n\x02id\x18\x01 \x02(\r\x12\x0c\n\x04name\x18\x02 \x02(\t\x12\x1f\n\x04pose\x18\x03 \x02(\x0b\x32\x11.gazebo.msgs.Pose\x12\x0c\n\x04\x64\x65\x61\x64\x18\x04 \x01(\x08\"]\n\x0bRobotStates\x12\x1f\n\x04time\x18\x01 \x02(\x0b\x32\x11.gazebo.msgs.Time\x12-\n\x0brobot_state\x18\x02 \x03(\x0b\x32\x18.revolve.msgs.RobotState') + serialized_pb=_b('\n\x12robot_states.proto\x12\x0crevolve.msgs\x1a\ntime.proto\x1a\npose.proto\"m\n\nRobotState\x12\n\n\x02id\x18\x01 \x02(\r\x12\x0c\n\x04name\x18\x02 \x02(\t\x12\x1f\n\x04pose\x18\x03 \x02(\x0b\x32\x11.gazebo.msgs.Pose\x12\x16\n\x0e\x62\x61ttery_charge\x18\x04 \x01(\x01\x12\x0c\n\x04\x64\x65\x61\x64\x18\x05 \x01(\x08\"]\n\x0bRobotStates\x12\x1f\n\x04time\x18\x01 \x02(\x0b\x32\x11.gazebo.msgs.Time\x12-\n\x0brobot_state\x18\x02 \x03(\x0b\x32\x18.revolve.msgs.RobotState') , dependencies=[time__pb2.DESCRIPTOR,pose__pb2.DESCRIPTOR,]) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) @@ -41,42 +42,49 @@ has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='name', full_name='revolve.msgs.RobotState.name', index=1, number=2, type=9, cpp_type=9, label=2, has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='pose', full_name='revolve.msgs.RobotState.pose', index=2, number=3, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( - name='dead', full_name='revolve.msgs.RobotState.dead', index=3, - number=4, type=8, cpp_type=7, label=1, + name='battery_charge', full_name='revolve.msgs.RobotState.battery_charge', index=3, + number=4, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='dead', full_name='revolve.msgs.RobotState.dead', index=4, + number=5, type=8, cpp_type=7, label=1, has_default_value=False, default_value=False, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], oneofs=[ ], serialized_start=60, - serialized_end=145, + serialized_end=169, ) @@ -93,28 +101,28 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='robot_state', full_name='revolve.msgs.RobotStates.robot_state', index=1, number=2, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], oneofs=[ ], - serialized_start=147, - serialized_end=240, + serialized_start=171, + serialized_end=264, ) _ROBOTSTATE.fields_by_name['pose'].message_type = pose__pb2._POSE @@ -122,7 +130,6 @@ _ROBOTSTATES.fields_by_name['robot_state'].message_type = _ROBOTSTATE DESCRIPTOR.message_types_by_name['RobotState'] = _ROBOTSTATE DESCRIPTOR.message_types_by_name['RobotStates'] = _ROBOTSTATES -_sym_db.RegisterFileDescriptor(DESCRIPTOR) RobotState = _reflection.GeneratedProtocolMessageType('RobotState', (_message.Message,), dict( DESCRIPTOR = _ROBOTSTATE, diff --git a/pyrevolve/spec/msgs/sdf_body_analyze_pb2.py b/pyrevolve/spec/msgs/sdf_body_analyze_pb2.py index 154482fab6..6c0c413480 100644 --- a/pyrevolve/spec/msgs/sdf_body_analyze_pb2.py +++ b/pyrevolve/spec/msgs/sdf_body_analyze_pb2.py @@ -7,6 +7,7 @@ from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() @@ -19,10 +20,10 @@ name='sdf_body_analyze.proto', package='revolve.msgs', syntax='proto2', - serialized_options=None, serialized_pb=_b('\n\x16sdf_body_analyze.proto\x12\x0crevolve.msgs\x1a\x0evector3d.proto\"1\n\x07\x43ontact\x12\x12\n\ncollision1\x18\x01 \x02(\t\x12\x12\n\ncollision2\x18\x02 \x02(\t\"U\n\x0b\x42oundingBox\x12\"\n\x03min\x18\x01 \x02(\x0b\x32\x15.gazebo.msgs.Vector3d\x12\"\n\x03max\x18\x02 \x02(\x0b\x32\x15.gazebo.msgs.Vector3d\"n\n\x14\x42odyAnalysisResponse\x12.\n\x0b\x62oundingBox\x18\x01 \x01(\x0b\x32\x19.revolve.msgs.BoundingBox\x12&\n\x07\x63ontact\x18\x02 \x03(\x0b\x32\x15.revolve.msgs.Contact') , dependencies=[vector3d__pb2.DESCRIPTOR,]) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) @@ -40,21 +41,21 @@ has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='collision2', full_name='revolve.msgs.Contact.collision2', index=1, number=2, type=9, cpp_type=9, label=2, has_default_value=False, default_value=_b("").decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -78,21 +79,21 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='max', full_name='revolve.msgs.BoundingBox.max', index=1, number=2, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -116,21 +117,21 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='contact', full_name='revolve.msgs.BodyAnalysisResponse.contact', index=1, number=2, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -147,7 +148,6 @@ DESCRIPTOR.message_types_by_name['Contact'] = _CONTACT DESCRIPTOR.message_types_by_name['BoundingBox'] = _BOUNDINGBOX DESCRIPTOR.message_types_by_name['BodyAnalysisResponse'] = _BODYANALYSISRESPONSE -_sym_db.RegisterFileDescriptor(DESCRIPTOR) Contact = _reflection.GeneratedProtocolMessageType('Contact', (_message.Message,), dict( DESCRIPTOR = _CONTACT, diff --git a/pyrevolve/spec/msgs/spline_policy_pb2.py b/pyrevolve/spec/msgs/spline_policy_pb2.py index 5b367e44d1..59ca614984 100644 --- a/pyrevolve/spec/msgs/spline_policy_pb2.py +++ b/pyrevolve/spec/msgs/spline_policy_pb2.py @@ -7,6 +7,7 @@ from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() @@ -19,10 +20,10 @@ name='spline_policy.proto', package='revolve.msgs', syntax='proto2', - serialized_options=None, serialized_pb=_b('\n\x13spline_policy.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"M\n\x06Spline\x12\r\n\x05index\x18\x01 \x02(\x05\x12\x0c\n\x04size\x18\x02 \x02(\x05\x12&\n\x05param\x18\x05 \x03(\x0b\x32\x17.revolve.msgs.Parameter\"\x17\n\x06Policy\x12\r\n\x05index\x18\x01 \x03(\x05\"6\n\x0cModifyPolicy\x12\x11\n\tadd_point\x18\x01 \x03(\x01\x12\x13\n\x0binterpolate\x18\x02 \x03(\t') , dependencies=[parameter__pb2.DESCRIPTOR,]) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) @@ -40,28 +41,28 @@ has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='size', full_name='revolve.msgs.Spline.size', index=1, number=2, type=5, cpp_type=1, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='param', full_name='revolve.msgs.Spline.param', index=2, number=5, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -85,14 +86,14 @@ has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -116,21 +117,21 @@ has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), _descriptor.FieldDescriptor( name='interpolate', full_name='revolve.msgs.ModifyPolicy.interpolate', index=1, number=2, type=9, cpp_type=9, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + options=None), ], extensions=[ ], nested_types=[], enum_types=[ ], - serialized_options=None, + options=None, is_extendable=False, syntax='proto2', extension_ranges=[], @@ -144,7 +145,6 @@ DESCRIPTOR.message_types_by_name['Spline'] = _SPLINE DESCRIPTOR.message_types_by_name['Policy'] = _POLICY DESCRIPTOR.message_types_by_name['ModifyPolicy'] = _MODIFYPOLICY -_sym_db.RegisterFileDescriptor(DESCRIPTOR) Spline = _reflection.GeneratedProtocolMessageType('Spline', (_message.Message,), dict( DESCRIPTOR = _SPLINE, diff --git a/pyrevolve/tol/manage/world.py b/pyrevolve/tol/manage/world.py index 651ab01553..f9506e2eeb 100644 --- a/pyrevolve/tol/manage/world.py +++ b/pyrevolve/tol/manage/world.py @@ -176,6 +176,13 @@ async def insert_population(self, trees, poses): future = multi_future(futures) future.add_done_callback( lambda _: logger.info("Done inserting population.")) + + # Awaiting this immediately will lock the program + update_state_future = self.set_state_update_frequency( + freq=self.state_update_frequency + ) + await update_state_future + return future def to_sdfbot(self, robot, robot_name, initial_battery=0.0): From 2cbf923ce3ccdf870d4885b3819357838de3879e Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Tue, 28 Apr 2020 07:19:49 -0700 Subject: [PATCH 16/19] Removed legacy battery functions, but this will cause no change to the battery state when it is requested to change. Currectly this request is never made, but should be cleaned up in the future to set the battery state with the new battery approach. --- .../revolve/gazebo/plugin/RobotController.cpp | 22 +++++++++++++++++++ .../revolve/gazebo/plugin/RobotController.h | 10 --------- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 8ea3dbbec4..70da263bc7 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -163,6 +163,7 @@ void RobotController::UpdateBattery(ConstRequestPtr &_request) resp.set_id(_request->id()); resp.set_request(_request->request()); + /* if (_request->request() == "set_battery_level") { resp.set_response("success"); @@ -174,6 +175,7 @@ void RobotController::UpdateBattery(ConstRequestPtr &_request) ss << this->BatteryLevel(); resp.set_response(ss.str()); } + */ batterySetPub_->Publish(resp); } @@ -341,6 +343,26 @@ void RobotController::LoadBattery(const sdf::ElementPtr _sdf) } } +///////////////////////////////////////////////// +double RobotController::BatteryLevel() +{ + if (not batteryElem_ or not batteryElem_->HasElement("rv:level")) + { + return 0.0; + } + + return batteryElem_->GetElement("rv:level")->Get< double >(); +} + +///////////////////////////////////////////////// +void RobotController::SetBatteryLevel(double _level) +{ + if (batteryElem_ and batteryElem_->HasElement("rv:level")) + { + batteryElem_->GetElement("rv:level")->Set(_level); + } +} + ///////////////////////////////////////////////// void RobotController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) { diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.h b/cpprevolve/revolve/gazebo/plugin/RobotController.h index bff3c8307e..ac10ca9acc 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.h +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.h @@ -65,16 +65,6 @@ namespace revolve /// according to the update rate specified in the robot plugin. virtual void DoUpdate(const ::gazebo::common::UpdateInfo _info); - /// \brief Returns the battery level - /// \details Methods allows reading and writing the battery level in - /// the robot SDF. This is mostly useful for the `BatterySensor` to - /// obtain the battery state, and storing it in the SDF also means it - /// will be adequately backed up in an eventual snapshot. - double BatteryLevel(); - - /// \brief Sets the battery level if possible - void SetBatteryLevel(double _level); - /// \brief Request listener for battery update void UpdateBattery(ConstRequestPtr &_request); From d1a6c6ae57ff8f3295933c9e2ae8a1350f3bf0a7 Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Tue, 28 Apr 2020 07:26:14 -0700 Subject: [PATCH 17/19] Removed legacy battery functions, but this will cause no change to the battery state when it is requested to change. Currectly this request is never made, but should be cleaned up in the future to set the battery state with the new battery approach. --- .../revolve/gazebo/plugin/RobotController.cpp | 20 ------------------- 1 file changed, 20 deletions(-) diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 70da263bc7..4a23d8c120 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -343,26 +343,6 @@ void RobotController::LoadBattery(const sdf::ElementPtr _sdf) } } -///////////////////////////////////////////////// -double RobotController::BatteryLevel() -{ - if (not batteryElem_ or not batteryElem_->HasElement("rv:level")) - { - return 0.0; - } - - return batteryElem_->GetElement("rv:level")->Get< double >(); -} - -///////////////////////////////////////////////// -void RobotController::SetBatteryLevel(double _level) -{ - if (batteryElem_ and batteryElem_->HasElement("rv:level")) - { - batteryElem_->GetElement("rv:level")->Set(_level); - } -} - ///////////////////////////////////////////////// void RobotController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) { From 1627a118e04a737ef8ca99e2da9874e972b442c6 Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Wed, 13 May 2020 17:09:01 -0700 Subject: [PATCH 18/19] Processing pull request review comments on alignment, tabs and typo's. --- .../revolve/gazebo/brains/DifferentialCPG.cpp | 36 ++----------- .../revolve/gazebo/brains/Evaluator.cpp | 17 ++---- cpprevolve/revolve/gazebo/cmake_install.cmake | 54 ------------------- cpprevolve/revolve/gazebo/motors/Motor.h | 1 - .../revolve/gazebo/motors/MotorFactory.h | 1 + .../revolve/gazebo/motors/PositionMotor.cpp | 20 ++++--- .../revolve/gazebo/plugin/RobotController.cpp | 44 +++++++-------- experiments/bo_learner/GridSearch.py | 40 ++++++-------- experiments/bo_learner/manager.py | 5 +- experiments/bo_learner/yaml/babyA.yaml | 8 +-- experiments/bo_learner/yaml/babyB.yaml | 4 +- experiments/bo_learner/yaml/babyC.yaml | 4 +- experiments/bo_learner/yaml/gecko12.yaml | 4 +- experiments/bo_learner/yaml/gecko17.yaml | 4 +- experiments/bo_learner/yaml/gecko7.yaml | 4 +- experiments/bo_learner/yaml/spider13.yaml | 4 +- experiments/bo_learner/yaml/spider17.yaml | 4 +- experiments/bo_learner/yaml/spider9.yaml | 4 +- experiments/examples/manager.py | 5 +- experiments/examples/yaml/spider.yaml | 5 +- pyrevolve/util/supervisor/supervisor_multi.py | 3 +- 21 files changed, 88 insertions(+), 183 deletions(-) delete mode 100644 cpprevolve/revolve/gazebo/cmake_install.cmake diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 4ca503c999..2e0b5a7706 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -64,9 +64,6 @@ using Init_t = limbo::init::LHS; using Kernel_t = limbo::kernel::MaternFiveHalves; using GP_t = limbo::model::GP; - - - /** * Constructor for DifferentialCPG class. * @@ -285,10 +282,8 @@ 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()) { std::cout << "§yes§"; @@ -298,11 +293,9 @@ DifferentialCPG::DifferentialCPG( else std::cout << "§no§\n"; + std::system(("mkdir -p " + this->directory_name).c_str()); - - std::system(("mkdir -p " + this->directory_name).c_str()); - - // Initialise array of neuron states for Update() methodc + // Initialise array of neuron states for Update() method this->next_state = new double[this->neurons.size()]; this->n_weights = (int)(this->connections.size()/2) + this->n_motors; @@ -383,21 +376,9 @@ DifferentialCPG::~DifferentialCPG() /** * Dummy function for limbo */ - - struct DifferentialCPG::evaluation_function{ // Number of input dimension (samples.size()) - // spider9:18, - // spider13:26, - // spider17:34, - // gecko7:13, - // gecko12:23, - // gecko17:33, - // babyA:16, - // babyB:22, - // babyC:32, - // one+:12 - BO_PARAM(size_t, dim_in, 13); // CHANGETHIS + BO_PARAM(size_t, dim_in, 18); // number of dimensions of the fitness BO_PARAM(size_t, dim_out, 1); @@ -434,9 +415,6 @@ void DifferentialCPG::bo_init_sampling(){ Eigen::VectorXd init_sample(this->n_weights); // For all weights - srand((unsigned)time(NULL)); - // trash first one, because it could be the same and I do not know why - auto trash_first = rand(); for (size_t j = 0; j < this->n_weights; j++) { // Generate a random number in [0, 1]. Transform later @@ -484,9 +462,6 @@ void DifferentialCPG::bo_init_sampling(){ Eigen::VectorXd init_sample(this->n_weights); // For all dimensions - srand((unsigned)time(NULL)); - // trash first one, because it could be the same and I do not know why - auto trash_first = rand(); for (size_t j = 0; j < this->n_weights; j++) { // Take a LHS @@ -668,7 +643,6 @@ void DifferentialCPG::bo_step(){ limbo::acquifun>> boptimizer; // Optimize. Pass dummy evaluation function and observations . - boptimizer.optimize(DifferentialCPG::evaluation_function(), this->samples, this->observations); @@ -846,7 +820,7 @@ void DifferentialCPG::Update( { std::cout << std::endl << "I am finished " << std::endl; } -// std::exit(0); + // std::exit(0); } // Evaluation policy here diff --git a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp index 03e1864ecf..f4be3f9e75 100644 --- a/cpprevolve/revolve/gazebo/brains/Evaluator.cpp +++ b/cpprevolve/revolve/gazebo/brains/Evaluator.cpp @@ -35,6 +35,7 @@ double Evaluator::measure_distance( return std::sqrt(std::pow(_pose1.Pos().X() - _pose2.Pos().X(), 2) + std::pow(_pose1.Pos().Y() - _pose2.Pos().Y(), 2)); } + ///////////////////////////////////////////////// Evaluator::Evaluator(std::shared_ptr<::revolve::gazebo::Battery> battery, const double _evaluationRate, @@ -71,7 +72,6 @@ void Evaluator::Reset() double Evaluator::Fitness() { double fitness_value = 0.0; - double speed; if(this->locomotion_type == "gait") { double dS; @@ -83,6 +83,7 @@ double Evaluator::Fitness() } else if (this->locomotion_type == "directed") { + this->step_poses.push_back(this->current_position_); //step_poses: x y z roll pitch yaw for (int i=1; i < this->step_poses.size(); i++) @@ -100,13 +101,8 @@ double Evaluator::Fitness() } coordinates << std::fixed << pose_i.Pos().X() << " " << pose_i.Pos().Y() << std::endl; } - // save the speed to speed.txt - std::ofstream speeds; - speeds.open(this->directory_name + "/speed.txt",std::ios::app); - speed = this->path_length / evaluation_rate_; - speeds << speed << std::endl; - ////********** directed locomotion fitness function **********//// + ////********** directed locomotion fitness function **********//// //directions(forward) of heads are the orientation(+x axis) - 1.570796 double beta0 = this->start_position_.Rot().Yaw()- M_PI/2.0; if (beta0 < - M_PI) //always less than pi (beta0 + max(40degree) < pi) @@ -191,11 +187,6 @@ double Evaluator::Fitness() } coordinates << std::fixed << pose_i.Pos().X() << " " << pose_i.Pos().Y() << std::endl; } - // save the speed to speed.txt - std::ofstream speeds; - speeds.open(this->directory_name + "/speed.txt",std::ios::app); - speed = this->path_length / evaluation_rate_; - speeds << speed << std::endl; ////********** directed locomotion fitness function **********//// //directions(forward) of heads are the orientation(+x axis) - 1.570796 @@ -384,7 +375,7 @@ double Evaluator::Fitness() std::cout << "fitness: " << fitness_value << "\n"; this->battery_->current_charge = 0; /// changing its charge to 0 to simulate the reseting of the robot } - exit(0); + exit(0); return fitness_value; } diff --git a/cpprevolve/revolve/gazebo/cmake_install.cmake b/cpprevolve/revolve/gazebo/cmake_install.cmake deleted file mode 100644 index 3eee625be8..0000000000 --- a/cpprevolve/revolve/gazebo/cmake_install.cmake +++ /dev/null @@ -1,54 +0,0 @@ -# Install script for directory: /Users/roy/projects/revolve/cpprevolve/revolve/gazebo - -# Set the install prefix -if(NOT DEFINED CMAKE_INSTALL_PREFIX) - set(CMAKE_INSTALL_PREFIX "/usr/local") -endif() -string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") - -# Set the install configuration name. -if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - if(BUILD_TYPE) - string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" - CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") - else() - set(CMAKE_INSTALL_CONFIG_NAME "Debug") - endif() - message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") -endif() - -# Set the component getting installed. -if(NOT CMAKE_INSTALL_COMPONENT) - if(COMPONENT) - message(STATUS "Install component: \"${COMPONENT}\"") - set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") - else() - set(CMAKE_INSTALL_COMPONENT) - endif() -endif() - -# Is this installation the result of a crosscompile? -if(NOT DEFINED CMAKE_CROSSCOMPILING) - set(CMAKE_CROSSCOMPILING "FALSE") -endif() - -if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) - file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/lib" TYPE STATIC_LIBRARY FILES "/Users/roy/projects/revolve/build/lib/librevolve-proto.a") - if(EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-proto.a" AND - NOT IS_SYMLINK "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-proto.a") - execute_process(COMMAND "/Library/Developer/CommandLineTools/usr/bin/ranlib" "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-proto.a") - endif() -endif() - -if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) - file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/lib" TYPE STATIC_LIBRARY FILES "/Users/roy/projects/revolve/build/lib/librevolve-gazebo.a") - if(EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-gazebo.a" AND - NOT IS_SYMLINK "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-gazebo.a") - execute_process(COMMAND "/Library/Developer/CommandLineTools/usr/bin/ranlib" "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/librevolve-gazebo.a") - endif() -endif() - -if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) - file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/include" TYPE DIRECTORY FILES "/Users/roy/projects/revolve/cpprevolve/revolve/gazebo/revolve" FILES_MATCHING REGEX "/[^/]*\\.h$") -endif() - diff --git a/cpprevolve/revolve/gazebo/motors/Motor.h b/cpprevolve/revolve/gazebo/motors/Motor.h index 6f003f114e..bf60426a77 100644 --- a/cpprevolve/revolve/gazebo/motors/Motor.h +++ b/cpprevolve/revolve/gazebo/motors/Motor.h @@ -26,7 +26,6 @@ #include #include - #include #include diff --git a/cpprevolve/revolve/gazebo/motors/MotorFactory.h b/cpprevolve/revolve/gazebo/motors/MotorFactory.h index e6109799ec..ac98da95f4 100644 --- a/cpprevolve/revolve/gazebo/motors/MotorFactory.h +++ b/cpprevolve/revolve/gazebo/motors/MotorFactory.h @@ -27,6 +27,7 @@ #include #include + namespace revolve { namespace gazebo diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index 5a260d858e..65da465ce4 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -135,13 +135,19 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) if (this->battery_) { - ::gazebo::physics::JointWrench jointWrench = this->joint_->GetForceTorque(0); - - // TODO find which axis to use local or global - // TODO check the power if it should be positive or negative - // TODO change this for now im using the absolute value of the power so it always decreases from the joint movements - double power = -abs(cmd * jointWrench.body1Torque.Length()); // TODO check which torque to use 1 or 2 - this->battery_->SetPowerLoad(this->consumerId_ , power); + ::gazebo::physics::JointWrench jointWrench = this->joint_->GetForceTorque(0); + + // TODO find which axis to use local or global + // TODO + // TODO change this for now im using the absolute value of the power so it always decreases from the joint movements + double power = cmd * jointWrench.body1Torque.Length(); + + // check the power if it should be negative + if (power > 0.0) + { + power = 0.0 + } + this->battery_->SetPowerLoad(this->consumerId_ , power); } this->joint_->SetParam("vel", 0, cmd); diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 4a23d8c120..96cce465ed 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -153,31 +153,31 @@ void RobotController::Load( ///////////////////////////////////////////////// void RobotController::UpdateBattery(ConstRequestPtr &_request) { - if (_request->data() not_eq this->model_->GetName() and - _request->data() not_eq this->model_->GetScopedName()) - { - return; - } + if (_request->data() not_eq this->model_->GetName() and + _request->data() not_eq this->model_->GetScopedName()) + { + return; + } - gz::msgs::Response resp; - resp.set_id(_request->id()); - resp.set_request(_request->request()); + gz::msgs::Response resp; + resp.set_id(_request->id()); + resp.set_request(_request->request()); - /* - if (_request->request() == "set_battery_level") - { - resp.set_response("success"); - this->SetBatteryLevel(_request->dbl_data()); - } - else - { - std::stringstream ss; - ss << this->BatteryLevel(); - resp.set_response(ss.str()); - } - */ + /* + if (_request->request() == "set_battery_level") + { + resp.set_response("success"); + this->SetBatteryLevel(_request->dbl_data()); + } + else + { + std::stringstream ss; + ss << this->BatteryLevel(); + resp.set_response(ss.str()); + } + */ - batterySetPub_->Publish(resp); + batterySetPub_->Publish(resp); } ///////////////////////////////////////////////// diff --git a/experiments/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index 2b1c04832c..cda3541657 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -17,42 +17,34 @@ from glob import glob from joblib import Parallel, delayed + # Parameters -min_lines = 1490 +min_lines = 1480 run_gazebo = False -n_runs = 20 # Naar 20 -n_jobs = 1 +n_runs = 15 # Naar 20 +n_jobs = 30 my_yaml_path = "experiments/bo_learner/yaml/" -yaml_model = "gecko7.yaml" # CHANGETHIS ! +yaml_model = "babyC.yaml" manager = "experiments/bo_learner/manager.py" -python_interpreter = "~/projects/revolve/.venv/bin/python3" - - - +python_interpreter = ".venv/bin/python3" search_space = { - 'load_brain': ["'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.31482.txt'", - "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.33971.txt'", - "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.34137.txt'", - "'/Users/roy/projects/revolve/output/brain_testing/directed_locomotion_runs/gecko7/best_brain_gecko7_1.44568.txt'"] - # 'evaluation_rate': [60], - # 'init_method': ["LHS"], - # 'verbose': [1], - # 'kernel_l': [0.1], - # 'acqui_ucb_alpha': [1.0], - # 'n_learning_iterations': [1450], - # 'n_init_samples': [50], - # 'kernel_l': [0.2], - # 'acqui_ucb_alpha': [3.0], - # 'n_learning_iterations': [950], - # 'n_init_samples': [20], + 'n_learning_iterations': [1500], + 'n_init_samples': [20], + 'evaluation_rate': [60], + 'verbose': [0], + 'kernel_sigma_sq': [1], + 'kernel_l': [0.02, 0.05, 0.1, 0.2], + 'acqui_ucb_alpha': [0.1, 0.3, 0.5, 1.0], + 'range_ub': [1.5], + 'signal_factor_all': [4.0], } print('search_space:', search_space) # You don't have to change this my_sub_directory = "yaml_temp/" output_path = "output/cpg_bo/main_" + str(round(time.time())) + "/" -start_port = 12000 # STEP 6 +start_port = 11000 finished = False # Make in revolve/build to allow runs from terminal diff --git a/experiments/bo_learner/manager.py b/experiments/bo_learner/manager.py index fe7e8b11f1..d66aa8f513 100755 --- a/experiments/bo_learner/manager.py +++ b/experiments/bo_learner/manager.py @@ -28,12 +28,11 @@ async def run(): # Load a robot from yaml robot = revolve_bot.RevolveBot() if settings.robot_yaml is None: - robot.load_file("experiments/bo_learner/yaml/spider9.yaml") + robot.load_file("experiments/bo_learner/yaml/spider.yaml") else: robot.load_file(settings.robot_yaml) robot.update_substrate() - # Start Simulator if settings.simulator_cmd != 'debug': simulator_supervisor = DynamicSimSupervisor( @@ -46,8 +45,6 @@ async def run(): ) await simulator_supervisor.launch_simulator(port=settings.port_start) - - # Connect to the simulator and pause world = await World.create(settings, world_address=('localhost', settings.port_start)) await world.pause(True) diff --git a/experiments/bo_learner/yaml/babyA.yaml b/experiments/bo_learner/yaml/babyA.yaml index e3353bbf18..e1420ea4e7 100644 --- a/experiments/bo_learner/yaml/babyA.yaml +++ b/experiments/bo_learner/yaml/babyA.yaml @@ -163,7 +163,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -184,10 +184,10 @@ brain: meta: robot_size: 26 run_analytics: "true" - n_learning_iterations: 0 - n_cooldown_iterations: 0 + n_learning_iterations: 1450 + n_cooldown_iterations: 1 reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 1 + verbose: 0 startup_time: 0 diff --git a/experiments/bo_learner/yaml/babyB.yaml b/experiments/bo_learner/yaml/babyB.yaml index 99edca2d34..1144e9f64f 100644 --- a/experiments/bo_learner/yaml/babyB.yaml +++ b/experiments/bo_learner/yaml/babyB.yaml @@ -195,7 +195,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -218,5 +218,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 1 + verbose: 0 startup_time: 0 diff --git a/experiments/bo_learner/yaml/babyC.yaml b/experiments/bo_learner/yaml/babyC.yaml index a8b56ebe9a..8448aa9939 100644 --- a/experiments/bo_learner/yaml/babyC.yaml +++ b/experiments/bo_learner/yaml/babyC.yaml @@ -309,7 +309,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -335,5 +335,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 1 + verbose: 0 startup_time: 0 diff --git a/experiments/bo_learner/yaml/gecko12.yaml b/experiments/bo_learner/yaml/gecko12.yaml index 9a39d5bb3c..bb6dfaaeab 100644 --- a/experiments/bo_learner/yaml/gecko12.yaml +++ b/experiments/bo_learner/yaml/gecko12.yaml @@ -226,7 +226,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -249,5 +249,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 1 + verbose: 0 startup_time: 0 diff --git a/experiments/bo_learner/yaml/gecko17.yaml b/experiments/bo_learner/yaml/gecko17.yaml index 926ee2d56c..1c32cf0ede 100644 --- a/experiments/bo_learner/yaml/gecko17.yaml +++ b/experiments/bo_learner/yaml/gecko17.yaml @@ -318,7 +318,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -341,5 +341,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 1 + verbose: 0 startup_time: 0 diff --git a/experiments/bo_learner/yaml/gecko7.yaml b/experiments/bo_learner/yaml/gecko7.yaml index 0546e82685..00b3eee25e 100644 --- a/experiments/bo_learner/yaml/gecko7.yaml +++ b/experiments/bo_learner/yaml/gecko7.yaml @@ -135,7 +135,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -158,5 +158,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 1 + verbose: 0 startup_time: 0 diff --git a/experiments/bo_learner/yaml/spider13.yaml b/experiments/bo_learner/yaml/spider13.yaml index 631c67dbf7..5fed980f4e 100644 --- a/experiments/bo_learner/yaml/spider13.yaml +++ b/experiments/bo_learner/yaml/spider13.yaml @@ -231,7 +231,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -254,5 +254,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 1 + verbose: 0 startup_time: 0 diff --git a/experiments/bo_learner/yaml/spider17.yaml b/experiments/bo_learner/yaml/spider17.yaml index ee8fe6fa4c..2cdae21c26 100644 --- a/experiments/bo_learner/yaml/spider17.yaml +++ b/experiments/bo_learner/yaml/spider17.yaml @@ -303,7 +303,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -326,5 +326,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: 1 + verbose: 0 startup_time: 0 diff --git a/experiments/bo_learner/yaml/spider9.yaml b/experiments/bo_learner/yaml/spider9.yaml index b1c729ee26..4f152974a0 100644 --- a/experiments/bo_learner/yaml/spider9.yaml +++ b/experiments/bo_learner/yaml/spider9.yaml @@ -155,7 +155,7 @@ brain: signal_factor_all: 4.0 signal_factor_mid: 2.5 signal_factor_left_right: 2.5 - range_lb: -1.5 + range_lb: -1.0 range_ub: 1.5 init_neuron_state: 0.707 learner: @@ -181,5 +181,5 @@ brain: reset_robot_position: "false" evaluation_rate: 60 output_directory: "" - verbose: true + verbose: 0 startup_time: 0 diff --git a/experiments/examples/manager.py b/experiments/examples/manager.py index 18afd516ce..00872116e3 100755 --- a/experiments/examples/manager.py +++ b/experiments/examples/manager.py @@ -9,9 +9,8 @@ from pyrevolve.SDF.math import Vector3 from pyrevolve import revolve_bot, parser from pyrevolve.tol.manage import World - -from pyrevolve.evolution import fitness from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor +from pyrevolve.evolution import fitness async def run(): @@ -40,6 +39,8 @@ async def run(): robot = revolve_bot.RevolveBot() robot.load_file(robot_file_path) robot.update_substrate() + # robot._brain = BrainRLPowerSplines() + # robot._brain = BrainRLPowerSplines() # Connect to the simulator and pause connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) diff --git a/experiments/examples/yaml/spider.yaml b/experiments/examples/yaml/spider.yaml index 0e80697d1a..ef973fb62c 100644 --- a/experiments/examples/yaml/spider.yaml +++ b/experiments/examples/yaml/spider.yaml @@ -158,7 +158,4 @@ brain: signal_factor_left_right : 2.5; abs_output_bound : 1.0; weights: [0.482167, 0.560357, 0.753772, 0.221536, 0.44513, 0.667353, 0.580933, 0.246228, 0.111797, - 0.110425, 0.667353, 0.519204, 0.11134, 0.667353, 0.70439, 0.000228624, 0.444673, 0.287837] -robot_config: - battery: - initial_charge: 1.0 \ No newline at end of file + 0.110425, 0.667353, 0.519204, 0.11134, 0.667353, 0.70439, 0.000228624, 0.444673, 0.287837] \ No newline at end of file diff --git a/pyrevolve/util/supervisor/supervisor_multi.py b/pyrevolve/util/supervisor/supervisor_multi.py index 3ab4b2ef63..d330309ca0 100644 --- a/pyrevolve/util/supervisor/supervisor_multi.py +++ b/pyrevolve/util/supervisor/supervisor_multi.py @@ -230,6 +230,7 @@ async def _launch_simulator(self, ready_str="World plugin loaded", output_tag="s """ Launches the simulator """ + self._logger.info("Launching the simulator...") gz_args = self.simulator_cmd + self.simulator_args snapshot_world = os.path.join( @@ -341,4 +342,4 @@ async def read_stderr(): await asyncio.sleep(0.1) raise RuntimeError(f'Process "{cmd[0]}" exited before it was ready. Exit code {process.returncode}') - return process \ No newline at end of file + return process From d4c01e279d1bba688fb86b29672615305c90e3cd Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Tue, 19 May 2020 03:13:57 -0700 Subject: [PATCH 19/19] Reviewed battery code --- .../revolve/gazebo/motors/PositionMotor.cpp | 17 +++++------------ .../revolve/gazebo/plugin/RobotController.cpp | 2 +- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index 65da465ce4..13c6e58dab 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -66,6 +66,7 @@ PositionMotor::PositionMotor( auto maxEffort = joint_->GetEffortLimit(0); joint_->SetParam("fmax", 0, maxEffort); + } ///////////////////////////////////////////////// @@ -129,26 +130,18 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) position += (position > 0 ? -2 * M_PI : 2 * M_PI); } - auto error = position - this->positionTarget_; auto cmd = this->pid_.Update(error, stepTime); // angular velocity TODO this is targeted velocity if (this->battery_) { ::gazebo::physics::JointWrench jointWrench = this->joint_->GetForceTorque(0); + // power is joule per second = N * m/s - // TODO find which axis to use local or global - // TODO - // TODO change this for now im using the absolute value of the power so it always decreases from the joint movements - double power = cmd * jointWrench.body1Torque.Length(); - - // check the power if it should be negative - if (power > 0.0) - { - power = 0.0 - } - this->battery_->SetPowerLoad(this->consumerId_ , power); + double angularVelocity = this->joint_->GetVelocity(0); + double power = -abs(angularVelocity * jointWrench.body1Torque.Length()); + this->battery_->SetPowerLoad(this->consumerId_ ,power); } this->joint_->SetParam("vel", 0, cmd); } diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index 96cce465ed..910d5453ba 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -17,7 +17,7 @@ * */ -#include +#include #include