diff --git a/cpprevolve/revolve/gazebo/CMakeLists.txt b/cpprevolve/revolve/gazebo/CMakeLists.txt index e01a3970fa..70345802ca 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..4432ca6021 --- /dev/null +++ b/cpprevolve/revolve/gazebo/battery/Battery.cpp @@ -0,0 +1,31 @@ +// +// Created by Roy Basmacier on 2019-07-09. +// + +#include "Battery.h" + +using namespace revolve::gazebo; + +Battery::Battery(double initial_charge) + : initial_charge(initial_charge), current_charge(initial_charge), time_init(std::to_string(time(0))), robot_name("") + {} + +void Battery::Update(double global_time, double delta_time) +{ + double sum = 0.0; + // std::cout << "battery: " << this->Voltage() << "V" << std::endl; + for (const auto &consumer: this->PowerLoads()) { +// std::cout << "comsumer: " << consumer.first << " -> " << consumer.second << std::endl; + sum += consumer.second; // TODO add constant so its linear + } + this->current_charge += sum * delta_time; // charge is measured in joules + + //TODO properly save battery data somewhere + /* + std::ofstream b_info_file; + b_info_file.open("output/cpg_bo/" + this->robot_name + "/" + this->time_init + "/battery.txt", std::ios_base::app); + if (b_info_file.fail()) + std::cout << "Failed to open: " << b_info_file.fail() << " " << "output/cpg_bo/" + this->robot_name + "/" + this->time_init + "/battery.txt" << std::endl; + b_info_file << global_time << " " << sum << " " << current_charge << std::endl; + */ +} diff --git a/cpprevolve/revolve/gazebo/battery/Battery.h b/cpprevolve/revolve/gazebo/battery/Battery.h new file mode 100644 index 0000000000..94ff253fbd --- /dev/null +++ b/cpprevolve/revolve/gazebo/battery/Battery.h @@ -0,0 +1,46 @@ +// +// Created by Roy Basmacier on 2019-07-09. +// + +#ifndef REVOLVE_BATTERY_H +#define REVOLVE_BATTERY_H + + +#include +#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 the time of initiation (for creating data files of battery delete later) + std::string time_init; + + std::string robot_name; + + friend class RobotController; + friend class Evaluator; + friend class DifferentialCPG; +}; + +} +} + +#endif //REVOLVE_BATTERY_H diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 99b75e8233..2e0b5a7706 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"); @@ -280,12 +283,15 @@ DifferentialCPG::DifferentialCPG( } // Create directory for output. - this->directory_name = controller->GetAttribute("output_directory")->GetAsString(); + // this->directory_name = controller->GetAttribute("output_directory")->GetAsString(); if(this->directory_name.empty()) { - this->directory_name = "output/cpg_bo/"; - this->directory_name += std::to_string(time(0)) + "/"; + std::cout << "§yes§"; + this->directory_name = "output/cpg_bo/" + this->robot->GetName() + "/"; // CHANGETHIS + this->directory_name += this->battery_->time_init + "/"; } + else + std::cout << "§no§\n"; std::system(("mkdir -p " + this->directory_name).c_str()); @@ -353,7 +359,7 @@ DifferentialCPG::DifferentialCPG( } // Initiate the cpp Evaluator - this->evaluator.reset(new Evaluator(this->evaluation_rate)); + this->evaluator.reset(new Evaluator(battery, this->evaluation_rate)); this->evaluator->directory_name = this->directory_name; } @@ -814,7 +820,7 @@ void DifferentialCPG::Update( { std::cout << std::endl << "I am finished " << std::endl; } - std::exit(0); + // std::exit(0); } // Evaluation policy here 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 e082cd1596..f4be3f9e75 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; @@ -47,7 +52,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 = "directed"; // {directed, gait, battery} // STEP 3 this->path_length = 0.0; } @@ -163,6 +168,214 @@ double Evaluator::Fitness() (dist_projection / (alpha + ksi) - penalty); fitness_value = fitness_direction; } + else if (this->locomotion_type == "battery") + { + 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; + } + + ////********** 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_->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); + + + + + // 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; + } + + + // 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)); + } + // ************************ + + + // + // + // f = P_initial + + +// 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_->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/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 a2b8e7f1dc..aa9f1c8545 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/Motor.h b/cpprevolve/revolve/gazebo/motors/Motor.h index 59764f49da..bf60426a77 100644 --- a/cpprevolve/revolve/gazebo/motors/Motor.h +++ b/cpprevolve/revolve/gazebo/motors/Motor.h @@ -28,6 +28,8 @@ #include #include +#include + namespace revolve { namespace gazebo @@ -83,6 +85,17 @@ namespace revolve /// \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 Pointer to the battery + protected: std::shared_ptr<::revolve::gazebo::Battery> battery_; + + /// \brief The id of the consumer + protected: 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 e512612ae8..9b6c9ea052 100644 --- a/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp +++ b/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp @@ -22,6 +22,8 @@ #include #include +#include + namespace gz = gazebo; using namespace revolve::gazebo; @@ -41,13 +43,20 @@ MotorPtr MotorFactory::Motor( const std::string &_type, const std::string &_partId, const std::string &_motorId, - const std::string &_coordinates) + const std::string &_coordinates, + std::shared_ptr<::revolve::gazebo::Battery> battery) { MotorPtr motor; if ("position" == _type) { - motor.reset(new PositionMotor(this->model_, _partId, _motorId, _motorSdf, _coordinates)); + PositionMotor *position_motor = new PositionMotor(this->model_, _partId, _motorId, _motorSdf, _coordinates); + + position_motor->battery_ = battery; + // adding consumer id to motor ptr from battery so each servo is a consumer of the battery + position_motor->consumerId_ = battery->AddConsumer(); + + motor.reset(position_motor); } else if ("velocity" == _type) { @@ -58,12 +67,12 @@ MotorPtr MotorFactory::Motor( } ///////////////////////////////////////////////// -MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf) +MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf, std::shared_ptr<::revolve::gazebo::Battery> battery) { auto coordinates = _motorSdf->GetAttribute("coordinates"); auto typeParam = _motorSdf->GetAttribute("type"); auto partIdParam = _motorSdf->GetAttribute("part_id"); -// auto partNameParam = _motorSdf->GetAttribute("part_name"); + // auto partNameParam = _motorSdf->GetAttribute("part_name"); auto idParam = _motorSdf->GetAttribute("id"); if (not typeParam or not partIdParam or not idParam) @@ -73,12 +82,12 @@ MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf) throw std::runtime_error("Motor error"); } -// auto partName = partNameParam->GetAsString(); + // auto partName = partNameParam->GetAsString(); auto partId = partIdParam->GetAsString(); auto type = typeParam->GetAsString(); auto id = idParam->GetAsString(); auto coord = coordinates->GetAsString(); - MotorPtr motor = this->Motor(_motorSdf, type, partId, id, coord); + MotorPtr motor = this->Motor(_motorSdf, type, partId, id, coord, battery); if (not motor) { @@ -88,3 +97,8 @@ MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf) return motor; } + +MotorFactoryPtr GetMotorFactory(::gazebo::physics::ModelPtr _model) +{ + return MotorFactoryPtr(new class MotorFactory(_model)); +} \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/motors/MotorFactory.h b/cpprevolve/revolve/gazebo/motors/MotorFactory.h index 530354909f..ac98da95f4 100644 --- a/cpprevolve/revolve/gazebo/motors/MotorFactory.h +++ b/cpprevolve/revolve/gazebo/motors/MotorFactory.h @@ -26,6 +26,7 @@ #include #include +#include namespace revolve { @@ -52,10 +53,11 @@ namespace revolve const std::string &_type, const std::string &_partId, const std::string &_motorId, - const std::string &_coordinates); + const std::string &_coordinates, + std::shared_ptr<::revolve::gazebo::Battery> battery); /// \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 5ddbbb2899..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); + } ///////////////////////////////////////////////// @@ -79,7 +80,8 @@ PositionMotor::~PositionMotor() = default; ///////////////////////////////////////////////// void PositionMotor::write( const double *outputs, - double /*step*/) + double /*step*/ + ) { // Just one network output, which is the first auto output = outputs[0]; @@ -129,7 +131,17 @@ void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) } auto error = position - this->positionTarget_; - auto cmd = this->pid_.Update(error, stepTime); + 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 + 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/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 566efa4b92..910d5453ba 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -17,13 +17,14 @@ * */ -#include +#include #include #include #include #include +#include #include "RobotController.h" @@ -36,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() { @@ -46,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_); } ///////////////////////////////////////////////// @@ -63,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", @@ -71,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." @@ -85,6 +124,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); @@ -97,9 +139,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); } @@ -117,24 +156,26 @@ void RobotController::UpdateBattery(ConstRequestPtr &_request) if (_request->data() not_eq this->model_->GetName() and _request->data() not_eq this->model_->GetScopedName()) { - return; + 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()); + resp.set_response("success"); + this->SetBatteryLevel(_request->dbl_data()); } else { - std::stringstream ss; - ss << this->BatteryLevel(); - resp.set_response(ss.str()); + std::stringstream ss; + ss << this->BatteryLevel(); + resp.set_response(ss.str()); } + */ batterySetPub_->Publish(resp); } @@ -155,7 +196,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"); } @@ -223,7 +264,7 @@ void RobotController::LoadBrain(const sdf::ElementPtr _sdf) } else if ("bo" == learner and "cpg" == controller_type) { - brain_.reset(new DifferentialCPG(this->model_, _sdf, motors_, sensors_)); + brain_.reset(new DifferentialCPG(this->model_, _sdf, motors_, sensors_, this->battery_)); } else if ("offline" == learner and "cpg" == controller_type) { @@ -265,10 +306,18 @@ 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_; if (brain_) brain_->Update(motors_, sensors_, currentTime, actuationTime_); + + if(battery_) + battery_->Update(currentTime, actuationTime_); } ///////////////////////////////////////////////// @@ -276,26 +325,161 @@ void RobotController::LoadBattery(const sdf::ElementPtr _sdf) { if (_sdf->HasElement("rv:battery")) { - this->batteryElem_ = _sdf->GetElement("rv:battery"); + sdf::ElementPtr batteryElem = _sdf->GetElement("rv: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_->robot_name = this->model_->GetName(); + } } ///////////////////////////////////////////////// -double RobotController::BatteryLevel() -{ - if (not batteryElem_ or not batteryElem_->HasElement("rv:level")) - { - return 0.0; +void RobotController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) { + + if (not this->robotStatesPubFreq_) { + return; } - return batteryElem_->GetElement("rv:level")->Get< double >(); + 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(); +// } } ///////////////////////////////////////////////// -void RobotController::SetBatteryLevel(double _level) -{ - if (batteryElem_ and batteryElem_->HasElement("rv:level")) +// 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") { - batteryElem_->GetElement("rv:level")->Set(_level); + 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 c72e36b3d2..ac10ca9acc 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.h +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.h @@ -32,6 +32,11 @@ #include "revolve/brains/controller/sensors/Sensor.h" #include "revolve/brains/controller/actuators/Actuator.h" +#include + +#include +#include + namespace revolve { namespace gazebo @@ -39,113 +44,137 @@ 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); - - /// \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); + virtual void DoUpdate(const ::gazebo::common::UpdateInfo _info); /// \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 + 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 */ -#endif /* REVOLVE_GAZEBO_PLUGIN_ROBOTCONTROLLER_H_ */ +#endif /* REVOLVE_GAZEBO_PLUGIN_ROBOTCONTROLLER_H_ */ \ No newline at end of file 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/bo_learner/GridSearch.py b/experiments/bo_learner/GridSearch.py index 71f8c0f2c9..cda3541657 100644 --- a/experiments/bo_learner/GridSearch.py +++ b/experiments/bo_learner/GridSearch.py @@ -27,6 +27,7 @@ yaml_model = "babyC.yaml" manager = "experiments/bo_learner/manager.py" python_interpreter = ".venv/bin/python3" + search_space = { 'n_learning_iterations': [1500], 'n_init_samples': [20], @@ -39,7 +40,7 @@ 'signal_factor_all': [4.0], } -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())) + "/" @@ -47,8 +48,8 @@ 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 ~/projects/revolve/ -DCMAKE_BUILD_TYPE="Release" -DLOCAL_GAZEBO_DIR=""') +# os.system("make -j4") def change_parameters(original_file, parameters): # Iterate over dictionary elements @@ -131,14 +132,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: @@ -151,32 +154,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.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 - ] + # 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 @@ -185,6 +188,7 @@ def run(i, sub_directory, model, params): # 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, @@ -209,9 +213,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") diff --git a/experiments/bo_learner/manager.py b/experiments/bo_learner/manager.py index bb095cd1d8..d66aa8f513 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,8 +33,20 @@ 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) + world = await World.create(settings, world_address=('localhost', settings.port_start)) await world.pause(True) await world.delete_model(robot.id) 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/examples/manager.py b/experiments/examples/manager.py index 69e45246a1..00872116e3 100755 --- a/experiments/examples/manager.py +++ b/experiments/examples/manager.py @@ -40,6 +40,7 @@ async def run(): 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)) @@ -58,5 +59,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..ef973fb62c 100644 --- a/experiments/examples/yaml/spider.yaml +++ b/experiments/examples/yaml/spider.yaml @@ -158,4 +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] + 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/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/experiments/roys_experiments/battery_chart.py b/experiments/roys_experiments/battery_chart.py new file mode 100644 index 0000000000..84707d1e9f --- /dev/null +++ b/experiments/roys_experiments/battery_chart.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/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/temp_charts.py b/experiments/roys_experiments/temp_charts.py new file mode 100644 index 0000000000..84707d1e9f --- /dev/null +++ b/experiments/roys_experiments/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/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 8167c19993..29f871fd5d 100644 --- a/pyrevolve/SDF/revolve_bot_sdf_builder.py +++ b/pyrevolve/SDF/revolve_bot_sdf_builder.py @@ -305,4 +305,11 @@ def _sdf_brain_plugin_conf( if robot_genome is not None: SDF.sub_element_text(config, 'rv:genome', str(robot_genome)) + #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/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): diff --git a/pyrevolve/util/supervisor/supervisor_multi.py b/pyrevolve/util/supervisor/supervisor_multi.py index 7b27ade29a..d330309ca0 100644 --- a/pyrevolve/util/supervisor/supervisor_multi.py +++ b/pyrevolve/util/supervisor/supervisor_multi.py @@ -285,7 +285,12 @@ def _disable_process_terminate_callbacks(self): self._process_terminated_futures.clear() async def _launch_with_ready_str(self, cmd, ready_str, env): - + """ + :param cmd: + :param ready_str: + :param env: + :return: + """ process = await asyncio.create_subprocess_exec( cmd[0], *cmd[1:], diff --git a/worlds/4_4m_walls.world b/worlds/4_4m_walls.world new file mode 100644 index 0000000000..8bc0b76181 --- /dev/null +++ b/worlds/4_4m_walls.world @@ -0,0 +1,168 @@ + + + + + 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 + + + + + + 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 + + + + + + + 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 + + + + + + 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 + + + + + + + -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 + + + + + +