├── .circleci └── config.yml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── Dockerfile ├── README.md ├── RELEASE.md ├── cpprevolve ├── CMakeLists.txt └── revolve │ ├── brains │ ├── CMakeLists.txt │ ├── controller │ │ ├── Controller.h │ │ ├── DifferentialCPG.cpp │ │ ├── DifferentialCPG.h │ │ ├── actuators │ │ │ └── Actuator.h │ │ └── sensors │ │ │ └── Sensor.h │ └── learner │ │ ├── BayesianOptimizer.cpp │ │ └── BayesianOptimizer.h │ ├── gazebo │ ├── CMakeLists.txt │ ├── Types.h │ ├── brains │ │ ├── Brain.h │ │ ├── BrainFactory.cpp │ │ ├── BrainFactory.h │ │ ├── Brains.h │ │ ├── DifferentialCPG.cpp │ │ ├── DifferentialCPG.h │ │ ├── DifferentialCPG_BO.h │ │ ├── Evaluator.cpp │ │ ├── Evaluator.h │ │ ├── NeuralNetwork.cpp │ │ ├── NeuralNetwork.h │ │ ├── RLPower.cpp │ │ ├── RLPower.h │ │ ├── ThymioBrain.cpp │ │ └── ThymioBrain.h │ ├── motors │ │ ├── JointMotor.cpp │ │ ├── JointMotor.h │ │ ├── Motor.cpp │ │ ├── Motor.h │ │ ├── MotorFactory.cpp │ │ ├── MotorFactory.h │ │ ├── Motors.h │ │ ├── PositionMotor.cpp │ │ ├── PositionMotor.h │ │ ├── VelocityMotor.cpp │ │ └── VelocityMotor.h │ ├── msgs │ │ ├── body.proto │ │ ├── model_inserted.proto │ │ ├── neural_net.proto │ │ ├── parameter.proto │ │ ├── robot.proto │ │ ├── robot_states.proto │ │ ├── sdf_body_analyze.proto │ │ └── spline_policy.proto │ ├── plugin │ │ ├── BodyAnalyzer.cpp │ │ ├── BodyAnalyzer.h │ │ ├── RobotController.cpp │ │ ├── RobotController.h │ │ ├── TorusWorld.cpp │ │ ├── TorusWorld.h │ │ ├── WorldController.cpp │ │ ├── WorldController.h │ │ ├── register_analyzer_plugin.cpp │ │ ├── register_robot_plugin.cpp │ │ ├── register_torus_world_plugin.cpp │ │ └── register_world_plugin.cpp │ ├── sensors │ │ ├── BatterySensor.cpp │ │ ├── BatterySensor.h │ │ ├── ImuSensor.cpp │ │ ├── ImuSensor.h │ │ ├── LightSensor.cpp │ │ ├── LightSensor.h │ │ ├── PointIntensitySensor.cpp │ │ ├── PointIntensitySensor.h │ │ ├── Sensor.cpp │ │ ├── Sensor.h │ │ ├── SensorFactory.cpp │ │ ├── SensorFactory.h │ │ ├── Sensors.h │ │ ├── TouchSensor.cpp │ │ ├── TouchSensor.h │ │ ├── VirtualSensor.cpp │ │ └── VirtualSensor.h │ └── util │ │ ├── YamlBodyParser.cpp │ │ └── YamlBodyParser.h │ └── raspberry │ ├── CMakeLists.txt │ ├── PIGPIOConnection.h │ ├── RaspController.cpp │ ├── RaspController.h │ ├── Servo.cpp │ ├── Servo.h │ ├── Timer.h │ ├── rasp_main.cpp │ └── robot_conf.yaml ├── docker ├── build_revolve.sh ├── make_docs.sh └── test_python.sh ├── docs ├── Doxyfile ├── doxygen.css ├── proto2cpp.py ├── revolve-logo-100x100.png └── revolve-logo.png ├── experiments ├── .gitignore ├── README.md ├── bo_learner │ ├── 3DPlot.py │ ├── AggregateGridSearches.py │ ├── BOTuningAnalysis.py │ ├── DeletePNGs.py │ ├── GridSearch manual.md │ ├── GridSearch.py │ ├── GridSearchAnalysis.py │ ├── RunAnalysisBO.py │ ├── manager.py │ ├── run_get_analytics.py │ └── yaml │ │ ├── babyA.yaml │ │ ├── babyB.yaml │ │ ├── babyC.yaml │ │ ├── gecko12.yaml │ │ ├── gecko17.yaml │ │ ├── gecko7.yaml │ │ ├── spider13.yaml │ │ ├── spider17.yaml │ │ └── spider9.yaml ├── examples │ ├── consolidate_experiments.py │ ├── manager.py │ ├── manager_pop.py │ ├── only_gazebo.py │ ├── run-experiments │ ├── sphere.sdf │ ├── stuck-experiments_watchman.py │ ├── summary_measures.R │ ├── tutorial1.py │ ├── tutorial2.py │ ├── tutorial3.py │ ├── watch_best_robots.py │ └── yaml │ │ ├── babyA.yaml │ │ ├── babyB.yaml │ │ ├── babyC.yaml │ │ ├── gecko.yaml │ │ ├── gecko12.yaml │ │ ├── gecko17.yaml │ │ ├── gecko7.yaml │ │ ├── robot_5.yaml │ │ ├── simple_robot.yaml │ │ ├── snake.yaml │ │ ├── spider.sdf.xml │ │ ├── spider.yaml │ │ ├── spider13.yaml │ │ ├── spider17.yaml │ │ └── spider9.yaml ├── karines_experiments │ ├── consolidate_experiments.py │ ├── manager_pop.py │ ├── manager_pop_lava.py │ ├── run-experiments │ ├── stuck-experiments_watchman.py │ ├── summary_measures.R │ └── watch_best_robots.py └── unmanaged │ ├── .gitignore │ ├── create_charts.py │ └── manager.py ├── models ├── rg_robot │ ├── meshes │ │ ├── ActiveCardanHinge_Servo_Holder.dae │ │ ├── ActiveHinge_Frame.dae │ │ ├── CoreComponent.dae │ │ ├── FixedBrick.dae │ │ ├── PassiveHinge.dae │ │ └── TouchSensor.dae │ ├── model.config │ └── robot.sdf ├── thymio │ ├── meshes │ │ ├── baterry.dae │ │ ├── chassis.dae │ │ ├── gripper.dae │ │ ├── raspberry_case.dae │ │ ├── thymio.dae │ │ └── wheel.dae │ ├── model.config │ └── model.sdf ├── tilted10 │ ├── _DS_Store │ ├── meshes │ │ ├── _DS_Store │ │ └── tilted10.dae │ └── model.config ├── tilted15 │ ├── _DS_Store │ ├── meshes │ │ └── tilted15.dae │ └── model.config ├── tilted5 │ ├── _DS_Store │ ├── meshes │ │ ├── _DS_Store │ │ └── tilted5.dae │ └── model.config ├── tol_arena │ ├── arena.sdf │ ├── meshes │ │ └── BirthClinic.dae │ └── model.config └── tol_ground │ ├── materials │ ├── scripts │ │ └── floor.material │ └── textures │ │ ├── asphalt.png │ │ ├── concrete1.png │ │ ├── grass.png │ │ ├── wood1.png │ │ ├── wood2.png │ │ └── wood3.png │ ├── model.config │ └── model.sdf ├── pyrevolve ├── SDF │ ├── __init__.py │ ├── geometry.py │ ├── inertial.py │ ├── joint.py │ ├── link.py │ ├── math │ │ ├── __init__.py │ │ ├── classes.py │ │ └── transformations.py │ ├── pose.py │ ├── revolve_bot_sdf_builder.py │ └── sensor.py ├── __init__.py ├── angle │ ├── README.md │ ├── __init__.py │ ├── evolve.py │ ├── generate.py │ ├── manage │ │ ├── __init__.py │ │ ├── robotmanager.py │ │ └── world.py │ ├── representation.py │ └── robogen │ │ ├── README.md │ │ ├── __init__.py │ │ ├── body_parts │ │ ├── __init__.py │ │ ├── active_cardan.py │ │ ├── active_hinge.py │ │ ├── active_rotator.py │ │ ├── active_wheel.py │ │ ├── active_wheg.py │ │ ├── cardan.py │ │ ├── core_component.py │ │ ├── fixed_brick.py │ │ ├── hinge.py │ │ ├── light_sensor.py │ │ ├── parametric_bar_joint.py │ │ ├── touch_sensor.py │ │ ├── util.py │ │ └── wheel.py │ │ ├── config.py │ │ ├── constants.py │ │ ├── spec │ │ ├── __init__.py │ │ ├── body.py │ │ └── robot.py │ │ └── util.py ├── build │ ├── __init__.py │ ├── sdf │ │ ├── __init__.py │ │ ├── battery.py │ │ ├── body │ │ │ ├── __init__.py │ │ │ ├── body_part.py │ │ │ ├── box.py │ │ │ ├── component.py │ │ │ ├── cylinder.py │ │ │ ├── exception.py │ │ │ └── joint.py │ │ ├── builder.py │ │ ├── motor.py │ │ ├── neural_net │ │ │ ├── __init__.py │ │ │ └── neuron.py │ │ └── sensor.py │ └── util.py ├── config.py ├── convert │ ├── __init__.py │ ├── decode.py │ ├── proto_to_yaml.py │ └── yaml.py ├── custom_logging │ ├── __init__.py │ └── logger.py ├── data_analisys │ ├── check_robot_collision.py │ └── visualize_robot.py ├── evolution │ ├── __init__.py │ ├── fitness.py │ ├── individual.py │ ├── pop_management │ │ ├── __init__.py │ │ ├── generational.py │ │ └── steady_state.py │ ├── population.py │ └── selection.py ├── examples │ ├── __init__.py │ ├── analyze_body.py │ ├── from_yaml.py │ └── generated_sdf.py ├── experiment_management.py ├── gazebo │ ├── __init__.py │ ├── analyze.py │ ├── connect.py │ └── manage │ │ ├── __init__.py │ │ └── world.py ├── generate │ └── __init__.py ├── genotype │ ├── __init__.py │ ├── genotype.py │ └── plasticoding │ │ ├── __init__.py │ │ ├── crossover │ │ ├── __init__.py │ │ ├── crossover.py │ │ └── standard_crossover.py │ │ ├── initialization.py │ │ ├── mutation │ │ ├── __init__.py │ │ ├── mutation.py │ │ └── standard_mutation.py │ │ └── plasticoding.py ├── revolve_bot │ ├── __init__.py │ ├── body.py │ ├── brain │ │ ├── __init__.py │ │ ├── base.py │ │ ├── bo_cpg.py │ │ ├── brain_nn.py │ │ └── rlpower_splines.py │ ├── measure │ │ ├── measure_body.py │ │ └── measure_brain.py │ ├── neural_net.py │ ├── render │ │ ├── __init__.py │ │ ├── brain_graph.py │ │ ├── canvas.py │ │ ├── grid.py │ │ └── render.py │ ├── revolve_bot.py │ └── revolve_module.py ├── spec │ ├── __init__.py │ ├── exception.py │ ├── implementation.py │ ├── msgs │ │ ├── __init__.py │ │ ├── body_pb2.py │ │ ├── model_inserted_pb2.py │ │ ├── neural_net_pb2.py │ │ ├── parameter_pb2.py │ │ ├── robot_pb2.py │ │ ├── robot_states_pb2.py │ │ ├── sdf_body_analyze_pb2.py │ │ └── spline_policy_pb2.py │ └── validate.py ├── tol │ ├── __init__.py │ ├── build │ │ ├── __init__.py │ │ └── builder.py │ ├── constants.py │ ├── logger.py │ ├── manage │ │ ├── __init__.py │ │ ├── measures.py │ │ ├── robotmanager.py │ │ └── world.py │ ├── scenery │ │ ├── __init__.py │ │ ├── birth_clinic.py │ │ └── wall.py │ ├── spec │ │ ├── __init__.py │ │ ├── body.py │ │ ├── brain.py │ │ └── robot.py │ └── util │ │ ├── __init__.py │ │ └── analyze.py └── util │ ├── __init__.py │ ├── asyncio_timer.py │ ├── functions.py │ ├── futures.py │ ├── supervisor │ ├── __init__.py │ ├── analyzer_queue.py │ ├── nbsr.py │ ├── simulator_queue.py │ ├── stream.py │ ├── supervisor.py │ ├── supervisor_collision.py │ └── supervisor_multi.py │ └── time.py ├── requirements.txt ├── revolve.py ├── test_py ├── __init__.py ├── convert │ ├── __init__.py │ ├── test_decode.py │ └── test_encode.py ├── generate │ ├── __init__.py │ └── test_revolvebot.py ├── plasticonding │ ├── __init__.py │ ├── genotype_176.txt │ ├── genotype_180.txt │ └── test_development.py ├── spec │ ├── __init__.py │ └── test_validate.py └── util │ ├── __init__.py │ └── test_time.py ├── test_robots.py ├── thirdparty └── PIGPIO │ ├── CMakeLists.txt │ ├── MakeRemote │ ├── Makefile │ ├── README │ ├── UNLICENCE │ ├── command.c │ ├── command.h │ ├── custom.cext │ ├── pig2vcd.1 │ ├── pig2vcd.c │ ├── pigpio.3 │ ├── pigpio.c │ ├── pigpio.h │ ├── pigpio.py │ ├── pigpio.service │ ├── pigpiod.1 │ ├── pigpiod.c │ ├── pigpiod_if.3 │ ├── pigpiod_if.c │ ├── pigpiod_if.h │ ├── pigpiod_if2.3 │ ├── pigpiod_if2.c │ ├── pigpiod_if2.h │ ├── pigs.1 │ ├── pigs.c │ ├── pigs.h │ ├── setup.py │ ├── x_pigpio.c │ ├── x_pigpio.py │ ├── x_pigpiod_if.c │ ├── x_pigpiod_if2.c │ ├── x_pigs │ └── x_pipe ├── tools ├── analyzer │ ├── analyzer-world.world │ └── run-analyzer ├── cmake_eclipse ├── code_check.sh ├── cpplint.py ├── models │ └── rg_robot │ │ ├── meshes │ │ ├── ActiveCardanHinge_Servo_Holder.dae │ │ ├── ActiveHinge_Frame.dae │ │ ├── CoreComponent.dae │ │ ├── FixedBrick.dae │ │ ├── PassiveHinge.dae │ │ └── TouchSensor.dae │ │ ├── model.config │ │ └── model.sdf ├── msg_check.py └── proto2python.sh └── worlds ├── benchmark.world ├── benchmark2.world ├── demo.world ├── idemo.world ├── offline-evolve.world ├── online-evolve.world ├── plane.ensure_contacts.world ├── plane.realtime.world ├── plane.unmanaged.world ├── plane.world ├── planobstacles.realtime.world ├── planobstacles.world ├── tilted10.realtime.world ├── tilted10.world ├── tilted15.realtime.world ├── tilted15.world ├── tilted5.realtime.world ├── tilted5.world └── viewer.world /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | *.dylib 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | /build/ 13 | /cmake-build-*/ 14 | /output*/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .coverage 43 | .coverage.* 44 | .cache 45 | nosetests.xml 46 | coverage.xml 47 | *,cover 48 | experiments/examples/output/ 49 | test.py 50 | 51 | # Translations 52 | *.mo 53 | *.pot 54 | 55 | # Django stuff: 56 | *.log 57 | 58 | # Sphinx documentation 59 | docs/_build/ 60 | 61 | # PyBuilder 62 | target/ 63 | 64 | # PyCharm project files 65 | .idea 66 | 67 | # Visual Studio Code 68 | /.vscode/ 69 | 70 | # Virtual env 71 | .venv*/ 72 | .python-version 73 | Pipfile 74 | Pipfile.lock 75 | 76 | # Revolve related 77 | *.sdf 78 | *.urdf -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thirdparty/limbo"] 2 | path = thirdparty/limbo 3 | url = https://github.com/resibots/limbo.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | 3 | project(revolve) 4 | 5 | if (BUILD_RASPBERRY) 6 | add_subdirectory(thirdparty/PIGPIO) 7 | endif() 8 | 9 | add_subdirectory(cpprevolve) -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM cigroup/gazebo:gazebo10-revolve 2 | 3 | # Dependencies 4 | RUN apt-get update && apt-get upgrade -y && apt-get install -y \ 5 | build-essential \ 6 | cmake \ 7 | git \ 8 | libgsl0-dev \ 9 | python3-pip \ 10 | libyaml-cpp-dev \ 11 | libcairo2-dev \ 12 | graphviz \ 13 | libeigen3-dev \ 14 | libnlopt-dev && \ 15 | apt-get clean && \ 16 | rm -rf /var/lib/apt/lists/* 17 | 18 | ADD . /revolve 19 | RUN /revolve/docker/build_revolve.sh 20 | -------------------------------------------------------------------------------- /cpprevolve/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMake Required Version 2 | cmake_minimum_required (VERSION 3.7.0) 3 | 4 | # Project name 5 | project (Revolve) 6 | set (CMAKE_CXX_STANDARD 11) 7 | 8 | # Include cmake subdirectories 9 | add_subdirectory(revolve/brains) 10 | 11 | if (NOT BUILD_ONLY_BRAIN) 12 | add_subdirectory(revolve/gazebo) 13 | endif() 14 | 15 | if (BUILD_RASPBERRY) 16 | add_subdirectory(revolve/raspberry) 17 | endif() 18 | -------------------------------------------------------------------------------- /cpprevolve/revolve/brains/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | file(GLOB_RECURSE 2 | CONTROLLER_SRCS 3 | controller/*.cpp 4 | controller/actuators/*.cpp 5 | controller/sensors/*.cpp 6 | ) 7 | file(GLOB_RECURSE 8 | LEARNER_SRCS 9 | learner/*.cpp 10 | ) 11 | 12 | # PKG-CONFIG 13 | find_package(PkgConfig REQUIRED) 14 | 15 | # Find Boost 16 | find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | # Find Eigen3 - A lightweight C++ template library for vector and matrix math 19 | find_package(Eigen3 REQUIRED) 20 | 21 | # Find NLOpt - Non Linear Optimization 22 | pkg_check_modules(NLOpt REQUIRED nlopt>=2.4) 23 | 24 | # Find Limbo - LIbrary for Model-Based Optimization 25 | set(LIMBO_DIR ${CMAKE_SOURCE_DIR}/thirdparty/limbo) 26 | set(LIMBO_DEFINES USE_NLOPT) 27 | 28 | add_library(revolve-controllers SHARED ${CONTROLLER_SRCS}) 29 | add_library(revolve-learners SHARED ${LEARNER_SRCS}) 30 | 31 | target_include_directories(revolve-controllers 32 | PUBLIC ${EIGEN3_INCLUDE_DIR} 33 | PUBLIC ${Boost_INCLUDE_DIRS}) 34 | 35 | target_include_directories(revolve-learners 36 | PUBLIC ${Boost_INCLUDE_DIRS} 37 | PUBLIC ${LIMBO_DIR}/src 38 | PUBLIC ${NLOpt_INCLUDE_DIRS}) 39 | 40 | target_include_directories(revolve-learners 41 | PUBLIC ${NLOpt_LIBRARY_DIRS}) 42 | 43 | install(TARGETS revolve-controllers revolve-learners 44 | RUNTIME DESTINATION bin 45 | LIBRARY DESTINATION lib) -------------------------------------------------------------------------------- /cpprevolve/revolve/brains/controller/Controller.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #ifndef REVOLVE_CONTROLLER_H 6 | #define REVOLVE_CONTROLLER_H 7 | 8 | #include "actuators/Actuator.h" 9 | #include "sensors/Sensor.h" 10 | #include 11 | #include 12 | 13 | namespace revolve 14 | { 15 | 16 | class Controller 17 | { 18 | public: 19 | /// \brief Constructor 20 | explicit Controller() {} 21 | 22 | /// \brief Deconstructor 23 | virtual ~Controller() {} 24 | 25 | virtual void update( 26 | const std::vector< std::unique_ptr< Actuator > > &_actuators, 27 | const std::vector< std::unique_ptr< Sensor > > &_sensors, 28 | const double _time, 29 | const double _step 30 | ) = 0; 31 | }; 32 | 33 | } 34 | 35 | 36 | #endif //REVOLVE_CONTROLLER_H 37 | -------------------------------------------------------------------------------- /cpprevolve/revolve/brains/controller/actuators/Actuator.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #ifndef REVOLVE_ACTUATOR_H 6 | #define REVOLVE_ACTUATOR_H 7 | 8 | #include 9 | 10 | namespace revolve { 11 | 12 | class Actuator 13 | { 14 | public: 15 | explicit Actuator(unsigned int n_outputs, double x, double y, double z) 16 | : Actuator(n_outputs, {x,y,z}) 17 | {} 18 | explicit Actuator(unsigned int n_outputs, const std::tuple coordinates) 19 | : _n_outputs(n_outputs) 20 | , coordinates(coordinates) 21 | {} 22 | 23 | inline double coordinate_x() const { return std::get<0>(this->coordinates); } 24 | inline double coordinate_y() const { return std::get<1>(this->coordinates); } 25 | inline double coordinate_z() const { return std::get<2>(this->coordinates); } 26 | 27 | virtual void write(const double *output, double step) = 0; 28 | 29 | inline unsigned int n_outputs() const {return this->_n_outputs;} 30 | 31 | private: 32 | const unsigned int _n_outputs; 33 | const std::tuple coordinates; 34 | }; 35 | 36 | } 37 | 38 | 39 | #endif //REVOLVE_ACTUATOR_H 40 | -------------------------------------------------------------------------------- /cpprevolve/revolve/brains/controller/sensors/Sensor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #ifndef REVOLVE_SENSOR_H 6 | #define REVOLVE_SENSOR_H 7 | 8 | namespace revolve { 9 | 10 | class Sensor 11 | { 12 | public: 13 | explicit Sensor(unsigned int n_inputs) 14 | : _n_inputs(n_inputs) 15 | {} 16 | 17 | /// \brief Read the value of the sensor into the 18 | /// \param[in] _input: array. 19 | /// \brief[in,out] _input Input value to write on 20 | virtual void read(double *input) = 0; 21 | 22 | inline unsigned int n_inputs() const {return this->_n_inputs;} 23 | 24 | private: 25 | const unsigned int _n_inputs; 26 | }; 27 | 28 | } 29 | 30 | #endif //REVOLVE_SENSOR_H 31 | -------------------------------------------------------------------------------- /cpprevolve/revolve/brains/learner/BayesianOptimizer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #include "BayesianOptimizer.h" 6 | -------------------------------------------------------------------------------- /cpprevolve/revolve/brains/learner/BayesianOptimizer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #ifndef REVOLVE_BAYESIANOPTIMIZER_H 6 | #define REVOLVE_BAYESIANOPTIMIZER_H 7 | 8 | 9 | class BayesianOptimizer { 10 | 11 | }; 12 | 13 | 14 | #endif //REVOLVE_BAYESIANOPTIMIZER_H 15 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/Types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: TODO: 16 | * Author: Elte Hupkes 17 | * 18 | */ 19 | 20 | #ifndef REVOLVE_GZ_MODEL_TYPES_H_ 21 | #define REVOLVE_GZ_MODEL_TYPES_H_ 22 | 23 | #include 24 | #include 25 | 26 | namespace revolve 27 | { 28 | namespace gazebo 29 | { 30 | class Motor; 31 | 32 | class VirtualSensor; 33 | 34 | class Brain; 35 | 36 | class MotorFactory; 37 | 38 | class SensorFactory; 39 | 40 | class Evaluator; 41 | 42 | typedef std::shared_ptr< Brain > BrainPtr; 43 | 44 | typedef std::shared_ptr< Motor > MotorPtr; 45 | 46 | typedef std::shared_ptr< VirtualSensor > SensorPtr; 47 | 48 | typedef std::shared_ptr< MotorFactory > MotorFactoryPtr; 49 | 50 | typedef std::shared_ptr< SensorFactory > SensorFactoryPtr; 51 | 52 | typedef std::shared_ptr< Evaluator > EvaluatorPtr; 53 | 54 | typedef std::vector< double > Spline; 55 | 56 | typedef std::vector< Spline > Policy; 57 | 58 | typedef std::shared_ptr< Policy > PolicyPtr; 59 | } 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/brains/Brain.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: Specifies a utility `Brain` base class. If your brain doesn't 16 | * fit this model, something else can easily be used by simply 17 | * ignoring the default brain behavior in the `RobotController`. 18 | * Author: Elte Hupkes 19 | * 20 | */ 21 | 22 | #ifndef REVOLVE_GAZEBO_BRAIN_BRAIN_H_ 23 | #define REVOLVE_GAZEBO_BRAIN_BRAIN_H_ 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | namespace revolve 35 | { 36 | namespace gazebo 37 | { 38 | class Brain 39 | { 40 | /// \brief Constructor 41 | public: explicit Brain() {} 42 | 43 | /// \brief Destructor 44 | public: virtual ~Brain() {} 45 | 46 | /// \brief Update step called for the brain. 47 | /// \param[in] _motors List of motors 48 | /// \param[in] _sensors List of sensors 49 | /// \param[in] _time Current simulation time 50 | /// \param[in] _step Actuation step size in seconds 51 | public: virtual void Update( 52 | const std::vector< MotorPtr > &_motors, 53 | const std::vector< SensorPtr > &_sensors, 54 | const double _time, 55 | const double _step) = 0; 56 | 57 | /// \brief Mutex for stepping / updating the network 58 | protected: boost::mutex networkMutex_; 59 | 60 | /// \brief Transport node 61 | protected: ::gazebo::transport::NodePtr node_; 62 | }; 63 | } /* namespace gazebo */ 64 | } /* namespace revolve */ 65 | 66 | #endif /* REVOLVE_GAZEBO_BRAIN_BRAIN_H_ */ 67 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/brains/BrainFactory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | * Description: TODO: 17 | * Author: Milan Jelisavcic 18 | * Date: December 23, 2018 19 | * 20 | */ 21 | 22 | #ifndef REVOLVE_GAZEBO_BRAINS_BRAINFACTORY_H_ 23 | #define REVOLVE_GAZEBO_BRAINS_BRAINFACTORY_H_ 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | namespace revolve 32 | { 33 | namespace gazebo 34 | { 35 | class BrainFactory 36 | { 37 | /// \brief Constructor 38 | /// \param[in] _model Model identifier 39 | public: explicit BrainFactory(::gazebo::physics::ModelPtr _model); 40 | 41 | /// \brief Destructor 42 | public: virtual ~BrainFactory(); 43 | 44 | /// \brief Returns a brain pointer reference from a brain element. 45 | /// This is the convenience wrapper over `create` that has required 46 | /// attributes already checked, usually you should override this when 47 | /// adding new brain types. 48 | public: virtual BrainPtr Brain( 49 | sdf::ElementPtr _brainSdf, 50 | const std::vector< MotorPtr > &_motors, 51 | const std::vector< SensorPtr > &_sensors); 52 | 53 | /// \brief Creates a brain for the given model for the given SDF element. 54 | public: virtual BrainPtr Create( 55 | sdf::ElementPtr _brainSdf, 56 | const std::vector< MotorPtr > &_motors, 57 | const std::vector< SensorPtr > &_sensors); 58 | 59 | /// \brief Internal reference to the robot model 60 | protected: ::gazebo::physics::ModelPtr model_; 61 | }; 62 | } 63 | } 64 | 65 | #endif // REVOLVE_GAZEBO_BRAINS_BRAINFACTORY_H_ 66 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/brains/Brains.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | * Description: TODO: 17 | * Author: Milan Jelisavcic 18 | * Date: Mar 16, 2015 19 | * 20 | */ 21 | 22 | #ifndef REVOLVE_GAZEBO_BRAINS_BRAINS_H_ 23 | #define REVOLVE_GAZEBO_BRAINS_BRAINS_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #endif // REVOLVE_GAZEBO_BRAINS_BRAINS_H_ 30 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/brains/ThymioBrain.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | * Description: TODO: 17 | * Author: Milan Jelisavcic 18 | * Date: 28/10/2018 19 | * 20 | */ 21 | 22 | #include 23 | #include 24 | 25 | #include "ThymioBrain.h" 26 | #include "../motors/Motor.h" 27 | #include "../sensors/Sensor.h" 28 | 29 | namespace gz = gazebo; 30 | 31 | using namespace revolve::gazebo; 32 | 33 | ThymioBrain::ThymioBrain( 34 | ::gazebo::physics::ModelPtr _model, 35 | sdf::ElementPtr /* _node */, 36 | std::vector< MotorPtr > &/* _motors */, 37 | std::vector< SensorPtr > &/* _sensors */) 38 | { 39 | std::cout << "Hello!" << std::endl; 40 | this->robot_ = _model; 41 | std::cout << this->robot_->GetName() << std::endl; 42 | } 43 | 44 | ThymioBrain::~ThymioBrain() = default; 45 | 46 | void ThymioBrain::Update( 47 | const std::vector< MotorPtr > &_motors, 48 | const std::vector< SensorPtr > &/* _sensors */, 49 | double /* _time */, 50 | double _step) 51 | { 52 | std::random_device rd; 53 | std::mt19937 mt(rd()); 54 | std::normal_distribution< double > dist(0, 1); 55 | 56 | auto numMotors = _motors.size(); 57 | auto *output = new double[numMotors]; 58 | for (size_t i = 0; i < numMotors; ++i) 59 | { 60 | output[i] += std::abs(dist(mt)); 61 | } 62 | 63 | std::cout << "Output: " << output[0] << std::endl; 64 | 65 | // Send new signals to the actuators 66 | auto p = 0; 67 | for (const auto &motor: _motors) 68 | { 69 | motor->Update(&output[p], _step); 70 | p += motor->Outputs(); 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/motors/JointMotor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Author: Elte Hupkes 16 | * Date: May 6, 2015 17 | * 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | namespace gz = gazebo; 25 | 26 | using namespace revolve::gazebo; 27 | 28 | ///////////////////////////////////////////////// 29 | JointMotor::JointMotor( 30 | ::gazebo::physics::ModelPtr _model, 31 | const std::string &_partId, 32 | const std::string &_motorId, 33 | sdf::ElementPtr _motor, 34 | const unsigned int _outputs) 35 | : Motor(_model, _partId, _motorId, _outputs) 36 | { 37 | if (not _motor->HasAttribute("joint")) 38 | { 39 | std::cerr << "JointMotor requires a `joint` attribute." << std::endl; 40 | throw std::runtime_error("Motor error"); 41 | } 42 | 43 | auto jointName = _motor->GetAttribute("joint")->GetAsString(); 44 | this->joint_ = _model->GetJoint(jointName); 45 | if (not this->joint_) 46 | { 47 | std::cerr << "Cannot locate joint motor `" << jointName << "`" << std::endl; 48 | throw std::runtime_error("Motor error"); 49 | } 50 | 51 | this->jointName_ = this->joint_->GetScopedName(); 52 | } 53 | 54 | ///////////////////////////////////////////////// 55 | JointMotor::~JointMotor() = default; 56 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/motors/JointMotor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: TODO: 16 | * Author: Elte Hupkes 17 | * Date: May 6, 2015 18 | * 19 | */ 20 | 21 | #ifndef REVOLVE_GAZEBO_MOTORS_JOINTMOTOR_H_ 22 | #define REVOLVE_GAZEBO_MOTORS_JOINTMOTOR_H_ 23 | 24 | #include 25 | 26 | #include 27 | 28 | namespace revolve 29 | { 30 | namespace gazebo 31 | { 32 | class JointMotor 33 | : public Motor 34 | { 35 | /// \brief Constructor 36 | /// \brief[in] _model Model identifier 37 | /// \brief[in] _partId Module identifier 38 | /// \brief[in] _motorId Motor identifier 39 | /// \brief[in] _outputs Number of motor outputs 40 | public: JointMotor( 41 | ::gazebo::physics::ModelPtr _model, 42 | const std::string &_partId, 43 | const std::string &_motorId, 44 | sdf::ElementPtr _motor, 45 | const unsigned int _outputs); 46 | 47 | /// \brief Destructor 48 | public: virtual ~JointMotor(); 49 | 50 | /// \brief The joint this motor is controlling 51 | protected: ::gazebo::physics::JointPtr joint_; 52 | 53 | /// \brief Scoped name of the controlled joint 54 | protected: std::string jointName_; 55 | }; 56 | } /* namespace gazebo */ 57 | } /* namespace revolve */ 58 | 59 | #endif /* REVOLVE_GAZEBO_MOTORS_JOINTMOTOR_H_ */ 60 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/motors/MotorFactory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: TODO: 16 | * Author: Elte Hupkes 17 | * Date: Mar 16, 2015 18 | * 19 | */ 20 | 21 | #ifndef REVOLVE_GAZEBO_MOTORS_MOTORFACTORY_H_ 22 | #define REVOLVE_GAZEBO_MOTORS_MOTORFACTORY_H_ 23 | 24 | #include 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace revolve 31 | { 32 | namespace gazebo 33 | { 34 | class MotorFactory 35 | { 36 | /// \brief Constructor 37 | /// \brief[in] _model Model identifier 38 | /// \brief[in] _partId Module identifier 39 | /// \brief[in] _motorId Motor identifier 40 | /// \brief[in] _outputs Number of motor outputs 41 | public: explicit MotorFactory(::gazebo::physics::ModelPtr model); 42 | 43 | /// \brief Destructor 44 | public: virtual ~MotorFactory(); 45 | 46 | /// \brief Returns a motor pointer instance from a motor element, part 47 | /// ID and type. This is the convenience wrapper over `create` that has 48 | /// required attributes already checked, usually you should override 49 | /// this when adding new motor types. 50 | public: virtual MotorPtr Motor( 51 | sdf::ElementPtr _motorSdf, 52 | const std::string &_type, 53 | const std::string &_partId, 54 | const std::string &_motorId); 55 | 56 | /// \brief Creates a motor for the given model for the given SDF element. 57 | public: virtual MotorPtr Create(sdf::ElementPtr _motorSdf); 58 | 59 | /// \brief Internal reference to the robot model 60 | protected: ::gazebo::physics::ModelPtr model_; 61 | }; 62 | } /* namespace gazebo */ 63 | } /* namespace revolve */ 64 | 65 | #endif /* REVOLVE_GAZEBO_MOTORS_MOTORFACTORY_H_ */ 66 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/motors/Motors.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: TODO: 16 | * Author: Elte Hupkes 17 | * 18 | */ 19 | 20 | #ifndef REVOLVE_GZ_MOTORS_H_ 21 | #define REVOLVE_GZ_MOTORS_H_ 22 | 23 | // Includes all motor types for convenience 24 | #include 25 | #include 26 | 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/msgs/body.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package revolve.msgs; 3 | import "parameter.proto"; 4 | 5 | // Protobuf message for a robot's body part based on Robogen design 6 | message BodyPart { 7 | // Unique identifier for a body part 8 | required string id = 1; 9 | // Type designator for a body part 10 | required string type = 2; 11 | // The x-coordinate in 2-dimensional plane 12 | required int32 x = 3; 13 | // The y-coordinate in 2-dimensional plane 14 | required int32 y = 4; 15 | // Orientation of a body part 16 | required double orientation = 5; 17 | // Designation of a child node pointer in a tree representation 18 | repeated BodyConnection child = 6; 19 | // Parameters for a ParametricBarJoint 20 | repeated Parameter param = 7; 21 | // Label for a body part 22 | optional string label = 8; 23 | } 24 | 25 | // BodyConnection message 26 | message BodyConnection { 27 | // Identifier of a source slot 28 | required int32 src_slot = 1; 29 | // Identifier of a destination slot 30 | required int32 dst_slot = 2; 31 | // BodyPart message 32 | required BodyPart part = 3; 33 | } 34 | 35 | // BodyPart message 36 | message Body { 37 | // Root BodyPart message 38 | required BodyPart root = 1; 39 | } 40 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/msgs/model_inserted.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package revolve.msgs; 3 | import "model.proto"; 4 | import "time.proto"; 5 | 6 | message ModelInserted { 7 | required gazebo.msgs.Time time = 1; 8 | required gazebo.msgs.Model model = 2; 9 | } 10 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/msgs/neural_net.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package revolve.msgs; 3 | import "parameter.proto"; 4 | 5 | message NeuralConnection { 6 | required string src = 1; 7 | required string dst = 2; 8 | required double weight = 3; 9 | } 10 | 11 | message Neuron { 12 | required string id = 1; 13 | required string layer = 2; 14 | required string type = 3; 15 | optional string partId = 4; 16 | repeated Parameter param = 5; 17 | } 18 | 19 | message NeuralNetwork { 20 | repeated Neuron neuron = 1; 21 | repeated NeuralConnection connection = 2; 22 | } 23 | 24 | message ModifyNeuralNetwork { 25 | repeated string remove_hidden = 1; 26 | repeated Neuron add_hidden = 2; 27 | repeated Neuron set_parameters = 4; 28 | repeated NeuralConnection set_weights = 3; 29 | } 30 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/msgs/parameter.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package revolve.msgs; 3 | 4 | message Parameter { 5 | required double value = 1; 6 | } 7 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/msgs/robot.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package revolve.msgs; 3 | import "body.proto"; 4 | import "neural_net.proto"; 5 | 6 | message Robot { 7 | required int32 id = 1; 8 | required Body body = 2; 9 | required NeuralNetwork brain = 3; 10 | } 11 | 12 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/msgs/robot_states.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package revolve.msgs; 3 | import "time.proto"; 4 | import "pose.proto"; 5 | 6 | message RobotState { 7 | required uint32 id = 1; 8 | required string name = 2; 9 | required gazebo.msgs.Pose pose = 3; 10 | optional bool dead = 4; 11 | } 12 | 13 | message RobotStates { 14 | required gazebo.msgs.Time time = 1; 15 | repeated RobotState robot_state = 2; 16 | } 17 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/msgs/sdf_body_analyze.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package revolve.msgs; 3 | import "vector3d.proto"; 4 | 5 | message Contact { 6 | required string collision1 = 1; 7 | required string collision2 = 2; 8 | } 9 | 10 | message BoundingBox { 11 | required gazebo.msgs.Vector3d min = 1; 12 | required gazebo.msgs.Vector3d max = 2; 13 | } 14 | 15 | message BodyAnalysisResponse { 16 | optional BoundingBox boundingBox = 1; 17 | repeated Contact contact = 2; 18 | } 19 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/msgs/spline_policy.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package revolve.msgs; 3 | import "parameter.proto"; 4 | 5 | message Spline { 6 | required int32 index = 1; 7 | required int32 size = 2; 8 | repeated Parameter param = 5; 9 | } 10 | 11 | message Policy { 12 | repeated int32 index = 1; 13 | } 14 | 15 | message ModifyPolicy { 16 | repeated double add_point = 1; 17 | repeated string interpolate = 2; 18 | } 19 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/plugin/TorusWorld.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 6/19/19. 3 | // 4 | 5 | #include "TorusWorld.h" 6 | 7 | using namespace revolve::gazebo; 8 | 9 | TorusWorld::TorusWorld() 10 | { 11 | 12 | } 13 | 14 | TorusWorld::~TorusWorld() 15 | { 16 | 17 | } 18 | 19 | void TorusWorld::Load(::gazebo::physics::WorldPtr _parent, sdf::ElementPtr _sdf) 20 | { 21 | 22 | } 23 | 24 | void TorusWorld::OnUpdate(const ::gazebo::common::UpdateInfo &_info) 25 | { 26 | 27 | } 28 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/plugin/TorusWorld.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 6/19/19. 3 | // 4 | 5 | #ifndef REVOLVE_TORUSWORLD_H 6 | #define REVOLVE_TORUSWORLD_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | namespace revolve { 15 | namespace gazebo { 16 | 17 | class TorusWorld : public ::gazebo::WorldPlugin { 18 | public: 19 | TorusWorld(); 20 | ~TorusWorld() override; 21 | 22 | virtual void Load( 23 | ::gazebo::physics::WorldPtr _parent, 24 | sdf::ElementPtr _sdf) override; 25 | 26 | virtual void OnUpdate(const ::gazebo::common::UpdateInfo &_info); 27 | 28 | private: 29 | 30 | // Pointer to the update event connection 31 | ::gazebo::event::ConnectionPtr onBeginUpdateConnection; 32 | ::gazebo::event::ConnectionPtr onEndUpdateConnection; 33 | }; 34 | 35 | } 36 | } 37 | 38 | 39 | #endif //REVOLVE_TORUSWORLD_H 40 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/plugin/register_analyzer_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Author: Elte Hupkes 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | using namespace gazebo; 23 | GZ_REGISTER_WORLD_PLUGIN(::revolve::gazebo::BodyAnalyzer) 24 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/plugin/register_robot_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Author: Milan Jelisavcic 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | using namespace gazebo; 23 | GZ_REGISTER_MODEL_PLUGIN(revolve::gazebo::RobotController) 24 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/plugin/register_torus_world_plugin.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 6/19/19. 3 | // 4 | 5 | #include 6 | #include "TorusWorld.h" 7 | 8 | using namespace gazebo; 9 | GZ_REGISTER_WORLD_PLUGIN(revolve::gazebo::TorusWorld) 10 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/plugin/register_world_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Author: Matteo De Carlo 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | using namespace gazebo; 23 | GZ_REGISTER_WORLD_PLUGIN(revolve::gazebo::WorldController) 24 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/sensors/BatterySensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Author: Elte Hupkes 16 | * 17 | */ 18 | 19 | #include 20 | 21 | #include "BatterySensor.h" 22 | 23 | namespace gz = gazebo; 24 | 25 | using namespace revolve::gazebo; 26 | 27 | ///////////////////////////////////////////////// 28 | BatterySensor::BatterySensor( 29 | ::gazebo::physics::ModelPtr _model, 30 | const std::string &_partId, 31 | const std::string &_sensorId) 32 | : VirtualSensor(_model, _partId, _sensorId, 1) 33 | { 34 | // Find the revolve plugin to get the battery data 35 | auto modelSdf = _model->GetSDF(); 36 | if (modelSdf->HasElement("plugin")) 37 | { 38 | auto pluginSdf = modelSdf->GetElement("plugin"); 39 | while (pluginSdf) 40 | { 41 | if (pluginSdf->HasElement("rv:robot_config")) 42 | { 43 | // Found revolve plugin 44 | auto settingsSdf = pluginSdf->GetElement("rv:robot_config"); 45 | if (settingsSdf->HasElement("rv:battery")) 46 | { 47 | this->battery_ = settingsSdf->GetElement("rv:battery"); 48 | } 49 | 50 | break; 51 | } 52 | pluginSdf = pluginSdf->GetNextElement("plugin"); 53 | } 54 | } 55 | } 56 | 57 | ///////////////////////////////////////////////// 58 | void BatterySensor::Read(double *_input) 59 | { 60 | _input[0] = this->battery_ and 61 | (this->battery_->HasElement("rv:level") ? 62 | this->battery_->GetElement("rv:level")->Get< double >() : 0.0); 63 | } 64 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/sensors/BatterySensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: Sensor that reads the battery state out of the model SDF 16 | * Author: Elte Hupkes 17 | * 18 | */ 19 | 20 | #ifndef REVOLVE_BATTERYSENSOR_H 21 | #define REVOLVE_BATTERYSENSOR_H 22 | 23 | #include 24 | 25 | #include "VirtualSensor.h" 26 | 27 | namespace revolve 28 | { 29 | namespace gazebo 30 | { 31 | class BatterySensor 32 | : public VirtualSensor 33 | { 34 | /// \brief Constructor 35 | /// \brief[in] _model Model identifier 36 | /// \brief[in] _partId Module identifier 37 | /// \brief[in] _sensorId Sensor identifier 38 | public: BatterySensor( 39 | ::gazebo::physics::ModelPtr _model, 40 | const std::string &_partId, 41 | const std::string &_sensorId); 42 | 43 | /// \brief Reads the battery value 44 | /// \param[in,out] _input: Input parameter of the sensor 45 | public: virtual void Read(double *_input); 46 | 47 | /// \brief SDF battery 48 | protected: sdf::ElementPtr battery_; 49 | }; 50 | } 51 | } 52 | 53 | #endif // REVOLVE_BATTERYSENSOR_H 54 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/sensors/ImuSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: TODO: 16 | * Author: Elte Hupkes 17 | * Date: Mar 24, 2015 18 | * 19 | */ 20 | 21 | #ifndef REVOLVE_GAZEBO_SENSORS_IMUSENSOR_H_ 22 | #define REVOLVE_GAZEBO_SENSORS_IMUSENSOR_H_ 23 | 24 | #include 25 | 26 | #include "Sensor.h" 27 | 28 | namespace revolve 29 | { 30 | namespace gazebo 31 | { 32 | class ImuSensor 33 | : public Sensor 34 | { 35 | /// \brief Constructor 36 | /// \brief[in] _model Model identifier 37 | /// \brief[in] _sensor Sensor identifier 38 | /// \brief[in] _partId Module identifier 39 | /// \brief[in] _sensorId Sensor identifier 40 | public: ImuSensor( 41 | ::gazebo::physics::ModelPtr _model, 42 | sdf::ElementPtr _sensor, 43 | const std::string &_partId, 44 | const std::string &_sensorId); 45 | 46 | /// \brief Destructor 47 | public: virtual ~ImuSensor(); 48 | 49 | /// \brief Read the value of this IMU sensor into the 50 | /// \param[in] _input: array. 51 | /// \brief[in,out] _input Input value to write on 52 | public: virtual void Read(double *_input); 53 | 54 | /// \brief Called when the IMU sensor is updated 55 | public: void OnUpdate(); 56 | 57 | /// \brief Sensor dynamically casted to correct type, so it needs to 58 | /// happen only once. 59 | private: ::gazebo::sensors::ImuSensorPtr castSensor_; 60 | 61 | /// \brief Pointer to the update connection 62 | private: ::gazebo::event::ConnectionPtr updateConnection_; 63 | 64 | /// \brief Last read sensor values 65 | private: double lastValues_[6]; 66 | }; 67 | } /* namespace gazebo */ 68 | } /* namespace revolve */ 69 | 70 | #endif // REVOLVE_GAZEBO_SENSORS_IMUSENSOR_H_ 71 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/sensors/Sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Author: Elte Hupkes 16 | * Date: Mar 24, 2015 17 | * 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | namespace gz = gazebo; 25 | 26 | using namespace revolve::gazebo; 27 | 28 | ///////////////////////////////////////////////// 29 | Sensor::Sensor( 30 | ::gazebo::physics::ModelPtr _model, 31 | sdf::ElementPtr _sensor, 32 | std::string _partId, 33 | std::string _sensorId, 34 | unsigned int _inputs) 35 | : VirtualSensor(_model, _partId, _sensorId, _inputs) 36 | { 37 | if (not _sensor->HasAttribute("sensor") or not _sensor->HasAttribute("link")) 38 | { 39 | std::cerr << "Sensor is missing required attributes (`link` or `sensor`)." 40 | << std::endl; 41 | throw std::runtime_error("Sensor error"); 42 | } 43 | 44 | auto sensorName = _sensor->GetAttribute("sensor")->GetAsString(); 45 | auto linkName = _sensor->GetAttribute("link")->GetAsString(); 46 | 47 | auto link = _model->GetLink(linkName); 48 | if (not link) 49 | { 50 | std::cerr << "Link '" << linkName << "' for sensor '" << sensorName 51 | << "' is not present in model." << std::endl; 52 | throw std::runtime_error("Sensor error"); 53 | } 54 | 55 | std::string scopedName = link->GetScopedName(true) + "::" + sensorName; 56 | this->sensor_ = gz::sensors::get_sensor(scopedName); 57 | 58 | if (not this->sensor_) 59 | { 60 | std::cerr << "Sensor with scoped name '" << scopedName 61 | << "' could not be found." << std::endl; 62 | throw std::runtime_error("Sensor error"); 63 | } 64 | } 65 | 66 | ///////////////////////////////////////////////// 67 | Sensor::~Sensor() = default; 68 | 69 | ///////////////////////////////////////////////// 70 | ::gazebo::sensors::SensorPtr Sensor::GzSensor() 71 | { 72 | return sensor_; 73 | } 74 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/sensors/Sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: Sensor class that is connected to an actual Gazebo sensor 16 | * Author: Elte Hupkes 17 | * 18 | */ 19 | 20 | #ifndef REVOLVE_GAZEBO_SENSORS_SENSOR_H_ 21 | #define REVOLVE_GAZEBO_SENSORS_SENSOR_H_ 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace revolve 28 | { 29 | namespace gazebo 30 | { 31 | class Sensor 32 | : public VirtualSensor 33 | { 34 | /// \brief Constructor 35 | /// \brief[in] _model Model identifier 36 | /// \brief[in] _sensor Sensor identifier 37 | /// \brief[in] _partId Module identifier 38 | /// \brief[in] _sensorId Sensor identifier 39 | /// \param[in] _inputs Number of inputs a sensor has 40 | public: Sensor( 41 | ::gazebo::physics::ModelPtr _model, 42 | sdf::ElementPtr _sensor, 43 | std::string _partId, 44 | std::string _sensorId, 45 | unsigned int _inputs); 46 | 47 | /// \brief Destructor 48 | public: virtual ~Sensor(); 49 | 50 | /// \return The attached Gazebo sensor 51 | public: ::gazebo::sensors::SensorPtr GzSensor(); 52 | 53 | /// \brief The actual sensor object this sensor is receiving input from 54 | protected: ::gazebo::sensors::SensorPtr sensor_; 55 | }; 56 | } /* namespace gazebo */ 57 | } /* namespace revolve */ 58 | 59 | #endif /* REVOLVE_GAZEBO_SENSORS_SENSOR_H_ */ 60 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/sensors/Sensors.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: TODO: 16 | * Author: Elte Hupkes 17 | * 18 | */ 19 | 20 | #ifndef REVOLVE_GZ_SENSORS_H_ 21 | #define REVOLVE_GZ_SENSORS_H_ 22 | 23 | // Includes all sensor types for convenience 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/sensors/TouchSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: TODO: 16 | * Author: Elte Hupkes 17 | * Date: Mar 27, 2015 18 | * 19 | */ 20 | 21 | #include 22 | 23 | #include 24 | 25 | namespace gz = gazebo; 26 | 27 | using namespace revolve::gazebo; 28 | 29 | ///////////////////////////////////////////////// 30 | TouchSensor::TouchSensor( 31 | ::gazebo::physics::ModelPtr _model, 32 | sdf::ElementPtr _sensor, 33 | std::string _partId, 34 | std::string _sensorId) 35 | : Sensor(_model, _sensor, _partId, _sensorId, 1) 36 | , lastValue_(false) 37 | { 38 | this->castSensor_ = std::dynamic_pointer_cast< gz::sensors::ContactSensor >( 39 | this->sensor_); 40 | 41 | if (not this->castSensor_) 42 | { 43 | std::cerr << "Creating a touch sensor with a non-contact sensor object" 44 | << std::endl; 45 | throw std::runtime_error("Sensor error"); 46 | } 47 | 48 | // Sensor must always update 49 | this->castSensor_->SetActive(true); 50 | 51 | // Add update connection that will produce new value 52 | this->updateConnection_ = this->sensor_->ConnectUpdated( 53 | std::bind(&TouchSensor::OnUpdate, this)); 54 | } 55 | 56 | ///////////////////////////////////////////////// 57 | TouchSensor::~TouchSensor() = default; 58 | 59 | ///////////////////////////////////////////////// 60 | void TouchSensor::OnUpdate() 61 | { 62 | auto contacts = this->castSensor_->Contacts(); 63 | this->lastValue_ = contacts.contact_size() > 0; 64 | } 65 | 66 | ///////////////////////////////////////////////// 67 | void TouchSensor::Read(double *_input) 68 | { 69 | _input[0] = this->lastValue_ ? 1 : 0; 70 | } 71 | -------------------------------------------------------------------------------- /cpprevolve/revolve/gazebo/sensors/VirtualSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Vrije Universiteit Amsterdam 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * Description: TODO: 16 | * Author: Elte Hupkes 17 | * 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | using namespace revolve::gazebo; 25 | 26 | ///////////////////////////////////////////////// 27 | VirtualSensor::VirtualSensor( 28 | ::gazebo::physics::ModelPtr _model, 29 | const std::string _partId, 30 | const std::string _sensorId, 31 | const unsigned int _inputs) 32 | : model_(_model) 33 | , partId_(_partId) 34 | , sensorId_(_sensorId) 35 | , inputs_(_inputs) 36 | { 37 | } 38 | 39 | ///////////////////////////////////////////////// 40 | VirtualSensor::~VirtualSensor() = default; 41 | 42 | ///////////////////////////////////////////////// 43 | unsigned int VirtualSensor::Inputs() 44 | { 45 | return this->inputs_; 46 | } 47 | 48 | ///////////////////////////////////////////////// 49 | std::string VirtualSensor::PartId() 50 | { 51 | return this->partId_; 52 | } 53 | 54 | ///////////////////////////////////////////////// 55 | std::string VirtualSensor::SensorId() 56 | { 57 | return this->sensorId_; 58 | } 59 | -------------------------------------------------------------------------------- /cpprevolve/revolve/raspberry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | message(WARNING "Building Raspberry code") 2 | 3 | # Find Yaml-cpp 4 | find_package(yaml-cpp REQUIRED) 5 | include_directories(${YAML_CPP_INCLUDE_DIR}) 6 | 7 | file(GLOB_RECURSE 8 | RASPBERRY_SRCS 9 | *.cpp 10 | ) 11 | 12 | add_executable(revolve-raspberry ${RASPBERRY_SRCS}) 13 | 14 | target_link_libraries(revolve-raspberry 15 | PUBLIC revolve-controllers 16 | PUBLIC pigpio_if2 17 | ${YAML_CPP_LIBRARIES} 18 | ) 19 | 20 | include_directories(${PIGPIO_HEADER_DIR}) 21 | 22 | install(TARGETS revolve-raspberry 23 | RUNTIME DESTINATION bin 24 | LIBRARY DESTINATION lib) 25 | -------------------------------------------------------------------------------- /cpprevolve/revolve/raspberry/PIGPIOConnection.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #ifndef REVOLVE_PIGPIOCONNECTION_H 6 | #define REVOLVE_PIGPIOCONNECTION_H 7 | 8 | extern "C" { 9 | #include "pigpiod_if2.h" 10 | } 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | class PIGPIOConnection { 17 | public: 18 | explicit PIGPIOConnection( 19 | const std::string &address = PI_DEFAULT_SOCKET_ADDR_STR, 20 | unsigned short port = PI_DEFAULT_SOCKET_PORT) 21 | { 22 | std::stringstream port_str; port_str << port; 23 | this->connection = pigpio_start(address.c_str(), port_str.str().c_str()); 24 | if (this->connection < 0) { 25 | throw std::runtime_error("connection unsuccessful"); 26 | } 27 | } 28 | 29 | ~PIGPIOConnection() { 30 | pigpio_stop(connection); 31 | } 32 | 33 | int set_pwm_frequency(unsigned user_gpio, unsigned frequency) { 34 | return set_PWM_frequency(connection, user_gpio, frequency); 35 | } 36 | 37 | int set_pwm_range(unsigned user_gpio, unsigned range) { 38 | return set_PWM_range(connection, user_gpio, range); 39 | } 40 | 41 | int set_pwm_dutycycle(unsigned user_gpio, unsigned dutycycle) { 42 | return set_PWM_dutycycle(connection, user_gpio, dutycycle); 43 | } 44 | 45 | private: 46 | int connection; 47 | }; 48 | 49 | 50 | #endif //REVOLVE_PIGPIOCONNECTION_H 51 | -------------------------------------------------------------------------------- /cpprevolve/revolve/raspberry/RaspController.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #include "RaspController.h" 6 | #include "../brains/controller/DifferentialCPG.h" 7 | #include 8 | 9 | using namespace revolve; 10 | 11 | RaspController::RaspController( 12 | std::vector > actuators, 13 | std::vector > sensors, 14 | const YAML::Node &conf) 15 | : revolve_controller(nullptr) 16 | , actuators(std::move(actuators)) 17 | , sensors(std::move(sensors)) 18 | { 19 | this->set_new_controller(conf); 20 | } 21 | 22 | RaspController::~RaspController() = default; 23 | 24 | #include 25 | #include 26 | 27 | void RaspController::update() 28 | { 29 | double step = this->timer.step(); 30 | double time = this->timer.elapsed(); 31 | if (step == 0) 32 | return; 33 | this->revolve_controller->update( 34 | this->actuators, 35 | this->sensors, 36 | time, 37 | step 38 | ); 39 | // std::this_thread::sleep_for(std::chrono::milliseconds(125)); 40 | } 41 | 42 | void RaspController::set_new_controller(const YAML::Node &conf) 43 | { 44 | std::string type = conf["type"].as(""); 45 | if (type.empty()) { 46 | throw std::runtime_error("Controller type not set"); 47 | } else if (type == "differential-cpg") { 48 | 49 | DifferentialCPG::ControllerParams params; 50 | params.reset_neuron_random = false; 51 | params.use_frame_of_reference = false; 52 | params.init_neuron_state = 0.707; 53 | params.range_ub = 1.0; 54 | params.signal_factor_all = 1.0; 55 | params.signal_factor_mid = 2.5; 56 | params.signal_factor_left_right = 2.5; 57 | params.abs_output_bound = 1.0; 58 | 59 | YAML::Node yaml_weights = conf["weights"]; 60 | for(const YAML::Node &weight: yaml_weights) { 61 | params.weights.emplace_back(weight.as()); 62 | } 63 | 64 | revolve_controller.reset( 65 | new DifferentialCPG(params,this->actuators) 66 | ); 67 | } else { 68 | throw std::runtime_error("Controller " + type + " not supported (yet)"); 69 | } 70 | } 71 | 72 | -------------------------------------------------------------------------------- /cpprevolve/revolve/raspberry/RaspController.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #ifndef REVOLVE_RASPCONTROLLER_H 6 | #define REVOLVE_RASPCONTROLLER_H 7 | 8 | #include 9 | #include 10 | #include "../brains/controller/Controller.h" 11 | #include "Timer.h" 12 | 13 | namespace revolve { 14 | 15 | class RaspController 16 | { 17 | public: 18 | explicit RaspController( 19 | std::vector > actuators, 20 | std::vector > sensors, 21 | const YAML::Node &conf); 22 | 23 | ~RaspController(); 24 | 25 | void update(); 26 | 27 | void set_new_controller(const YAML::Node &conf); 28 | 29 | private: 30 | std::unique_ptr revolve_controller; 31 | Timer timer; 32 | std::vector< std::unique_ptr< revolve::Actuator > > actuators; 33 | std::vector< std::unique_ptr< revolve::Sensor > > sensors; 34 | }; 35 | 36 | } 37 | 38 | 39 | #endif //REVOLVE_RASPCONTROLLER_H 40 | -------------------------------------------------------------------------------- /cpprevolve/revolve/raspberry/Servo.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #include "Servo.h" 6 | #include "PIGPIOConnection.h" 7 | 8 | using namespace revolve; 9 | 10 | #define POSITION_OFF 0 11 | #define POSITION_BEGIN 40 12 | #define POSITION_MIDDLE 75 13 | #define POSITION_END 110 14 | 15 | Servo::Servo(double coordinate_x, double coordinate_y, double coordinate_z, PIGPIOConnection *connection, unsigned short pin, unsigned int frequency, int range, bool inverse) 16 | : Actuator(1, coordinate_x, coordinate_y, coordinate_z) 17 | , pin(pin) 18 | , pigpio(connection) 19 | , frequency(frequency) 20 | , range(range) 21 | , inverse(inverse) 22 | { 23 | pigpio->set_pwm_frequency(pin, frequency); 24 | pigpio->set_pwm_range(pin, range); 25 | 26 | if (inverse) { 27 | minPWM = POSITION_END; 28 | maxPWM = POSITION_BEGIN; 29 | } else { 30 | minPWM = POSITION_BEGIN; 31 | maxPWM = POSITION_END; 32 | } 33 | } 34 | 35 | void Servo::move_to_position(double position) 36 | { 37 | if (position < -1.0) 38 | position = -1.0; 39 | else if (position > 1.0) 40 | position = 1.0; 41 | 42 | position *= 0.7; 43 | 44 | position = this->minPWM + (1 + position) * (this->maxPWM - this->minPWM) / 2; 45 | 46 | this->pigpio->set_pwm_dutycycle(this->pin, static_cast(position)); 47 | } 48 | 49 | void Servo::off() 50 | { 51 | this->pigpio->set_pwm_dutycycle(this->pin, POSITION_OFF); 52 | } 53 | 54 | void Servo::write(const double *output_vector, double step) 55 | { 56 | double output = output_vector[0]; 57 | this->move_to_position(output); 58 | } 59 | 60 | std::ostream &operator<<(std::ostream &os, Servo const &s) { 61 | return s.print(os); 62 | } 63 | -------------------------------------------------------------------------------- /cpprevolve/revolve/raspberry/Servo.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 14/06/19. 3 | // 4 | 5 | #ifndef REVOLVE_SERVO_H 6 | #define REVOLVE_SERVO_H 7 | 8 | #include "PIGPIOConnection.h" 9 | #include "../brains/controller/actuators/Actuator.h" 10 | #include 11 | 12 | namespace revolve { 13 | 14 | class Servo: public revolve::Actuator 15 | { 16 | public: 17 | explicit Servo( 18 | double coordinate_x, 19 | double coordinate_y, 20 | double coordinate_z, 21 | PIGPIOConnection *connection, 22 | unsigned short pin, 23 | unsigned int frequency=50, 24 | int range=1000, 25 | bool inverse=false 26 | ); 27 | 28 | ~Servo() { 29 | this->off(); 30 | } 31 | 32 | /*** 33 | * Sends signal to the engine to move to a specified position. 34 | * Position should be in range [-1, 1] with 0 being the middle. 35 | * @param position 36 | */ 37 | void move_to_position(double position); 38 | 39 | void center() { move_to_position(0); } 40 | 41 | void off(); 42 | 43 | virtual void write(const double *output, double step) override; 44 | 45 | std::ostream &print(std::ostream &os) const 46 | { 47 | os << "Servo pin:\t" << this->pin << std::endl; 48 | os << "\tcoordinates [" << this->coordinate_x() << ',' << this->coordinate_y() << ',' << this->coordinate_z() << ']' << std::endl; 49 | os << "\tfrequency:\t" << this->frequency << std::endl; 50 | os << "\trange: \t" << this->range << std::endl; 51 | os << "\tinverse: \t" << this->inverse; 52 | 53 | return os; 54 | } 55 | 56 | private: 57 | unsigned short pin; 58 | PIGPIOConnection *pigpio; 59 | unsigned int frequency; 60 | int range; 61 | bool inverse; 62 | float minPWM; 63 | float maxPWM; 64 | }; 65 | 66 | } 67 | 68 | std::ostream &operator<<(std::ostream &os, revolve::Servo const &s); 69 | 70 | #endif //REVOLVE_SERVO_H 71 | -------------------------------------------------------------------------------- /cpprevolve/revolve/raspberry/Timer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by matteo on 17/06/19. 3 | // 4 | 5 | #ifndef REVOLVE_TIMER_H 6 | #define REVOLVE_TIMER_H 7 | #include 8 | #include 9 | 10 | class Timer 11 | { 12 | public: 13 | Timer() 14 | : beg_(clock_::now()) 15 | , last_step_(beg_) 16 | {} 17 | void reset() { beg_ = clock_::now(); } 18 | double step() { 19 | std::chrono::time_point prev = last_step_; 20 | last_step_ = clock_::now(); 21 | return time_difference(prev, last_step_); 22 | } 23 | double elapsed() const { return time_difference(beg_, last_step_); } 24 | double elapsed_now() const { return time_difference(beg_, clock_::now()); } 25 | 26 | private: 27 | typedef std::chrono::high_resolution_clock clock_; 28 | typedef std::chrono::duration > second_; 29 | std::chrono::time_point beg_; 30 | std::chrono::time_point last_step_; 31 | 32 | static double time_difference(const std::chrono::time_point start, const std::chrono::time_point end) 33 | { 34 | return std::chrono::duration_cast 35 | (end - start).count(); 36 | } 37 | }; 38 | 39 | #endif //REVOLVE_TIMER_H 40 | -------------------------------------------------------------------------------- /docker/build_revolve.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # Build Revolve 5 | cd /revolve 6 | 7 | # use version 10 8 | sed -i 's/ find_package(gazebo 9 REQUIRED)/ find_package(gazebo 10 REQUIRED)/g' cpprevolve/revolve/gazebo/CMakeLists.txt 9 | 10 | mkdir -p build && cd build 11 | cmake .. -DCMAKE_BUILD_TYPE="Release" 12 | make -j4 13 | 14 | # Install the Python dependencies 15 | cd /revolve 16 | pip3 install -r requirements.txt 17 | -------------------------------------------------------------------------------- /docker/make_docs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # Build the documentation 5 | cd /revolve 6 | doxygen docs/Doxyfile 7 | 8 | # Push the documentation on `gh-pages` branch 9 | git config credential.helper 'cache --timeout=120' 10 | git config user.email "" 11 | git config user.name "Documentation bot" 12 | git add docs/ 13 | git commit -m "Update documentation via CircleCI [ci skip]" 14 | git push -q https://${GITHUB_PERSONAL_TOKEN}@github.com/ci-group/revolve.git gh-pages 15 | -------------------------------------------------------------------------------- /docker/test_python.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # Run Python unittests 5 | cd /revolve 6 | python3 -m unittest discover test_py/ 7 | 8 | -------------------------------------------------------------------------------- /docs/doxygen.css: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/docs/doxygen.css -------------------------------------------------------------------------------- /docs/revolve-logo-100x100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/docs/revolve-logo-100x100.png -------------------------------------------------------------------------------- /docs/revolve-logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/docs/revolve-logo.png -------------------------------------------------------------------------------- /experiments/.gitignore: -------------------------------------------------------------------------------- 1 | /default_experiment 2 | /*/data -------------------------------------------------------------------------------- /experiments/README.md: -------------------------------------------------------------------------------- 1 | # Experiments folder 2 | 3 | This folder contains all experimental managers, just run 4 | ``` 5 | python3 revolve.py --manager experiments/your_experintal_manager.py 6 | ``` 7 | to re-run the experiment. -------------------------------------------------------------------------------- /experiments/bo_learner/AggregateGridSearches.py: -------------------------------------------------------------------------------- 1 | import matplotlib 2 | from sys import platform 3 | if platform == "darwin": 4 | matplotlib.use("TkAgg") 5 | import matplotlib.pyplot as plt 6 | from glob import glob 7 | import numpy as np 8 | 9 | 10 | # Parameters 11 | path = "/home/maarten/Thesis_output/cpg_tuning/" 12 | my_size_x = 11 13 | my_size_y = 13 14 | var1 = "range_ub" 15 | var2 = "signal_factor" 16 | 17 | paths = glob(path + "*/") 18 | Z1 = np.loadtxt(paths[0] + "Z.txt", delimiter=",") 19 | Z2 = np.loadtxt(paths[1] + "Z.txt", delimiter=",") 20 | Z3 = np.loadtxt(paths[2] + "Z.txt", delimiter=",") 21 | Z4 = np.loadtxt(paths[3] + "Z.txt", delimiter=",") 22 | Z5 = np.loadtxt(paths[4] + "Z.txt", delimiter=",") 23 | Z6 = np.loadtxt(paths[5] + "Z.txt", delimiter=",") 24 | Z7 = np.loadtxt(paths[6] + "Z.txt", delimiter=",") 25 | Z8 = np.loadtxt(paths[7] + "Z.txt", delimiter=",") 26 | Z9 = np.loadtxt(paths[7] + "Z.txt", delimiter=",") 27 | Z = (Z1 + Z2 + Z3 + Z4 + Z5 + Z6 + Z7 + Z8 + Z9)/9.0 28 | 29 | X = np.loadtxt(paths[0] + "X.txt", delimiter=",") 30 | Y = np.loadtxt(paths[0] + "Y.txt", delimiter=",") 31 | 32 | # Create plot 33 | fig, ax = plt.subplots() 34 | cs = plt.contourf(X.tolist(),Y.tolist(),Z.tolist(),extend = "both", cmap = "hot_r", levels = 15) 35 | cs.changed() 36 | fig.colorbar(cs) 37 | plt.xlabel(var1) 38 | plt.ylabel(var2) 39 | plt.savefig(path + "cpg_combined.png") 40 | 41 | # Save XYZ 42 | np.savetxt(path + "X.txt", np.matrix(X), fmt='%f', delimiter = ",") 43 | np.savetxt(path + "Y.txt", np.matrix(Y), fmt='%f', delimiter = ",") 44 | np.savetxt(path + "Z.txt", np.matrix(Z), fmt='%f', delimiter = ",") 45 | -------------------------------------------------------------------------------- /experiments/bo_learner/DeletePNGs.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | import os 3 | 4 | path = "/home/gongjinlan/projects/revolve/output/cpg_bo/main_1559644358-BO-gecko7/" 5 | print(path) 6 | 7 | paths = glob(path + "*/*.png") 8 | print(len(paths)) 9 | 10 | for path in paths: 11 | os.remove(path) 12 | -------------------------------------------------------------------------------- /experiments/bo_learner/manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import sys 4 | import asyncio 5 | 6 | # Add `..` folder in search path 7 | current_dir = os.path.dirname(os.path.abspath(__file__)) 8 | newpath = os.path.join(current_dir, '..', '..') 9 | sys.path.append(newpath) 10 | 11 | from pygazebo.pygazebo import DisconnectError 12 | 13 | from pyrevolve import revolve_bot 14 | from pyrevolve import parser 15 | from pyrevolve.SDF.math import Vector3 16 | from pyrevolve.tol.manage import World 17 | 18 | 19 | async def run(): 20 | """ 21 | The main coroutine, which is started below. 22 | """ 23 | # Parse command line / file input arguments 24 | settings = parser.parse_args() 25 | 26 | # Load a robot from yaml 27 | robot = revolve_bot.RevolveBot() 28 | if settings.robot_yaml is None: 29 | robot.load_file("experiments/bo_learner/yaml/spider.yaml") 30 | else: 31 | robot.load_file(settings.robot_yaml) 32 | robot.update_substrate() 33 | 34 | # Connect to the simulator and pause 35 | world = await World.create(settings) 36 | await world.pause(True) 37 | 38 | await world.delete_model(robot.id) 39 | await asyncio.sleep(2.5) 40 | 41 | # Insert the robot in the simulator 42 | robot_manager = await world.insert_robot(robot, Vector3(0, 0, 0.025)) 43 | 44 | # Resume simulation 45 | await world.pause(False) 46 | 47 | # Start a run loop to do some stuff 48 | while True: 49 | await asyncio.sleep(5.0) 50 | 51 | 52 | def main(): 53 | def handler(loop, context): 54 | exc = context['exception'] 55 | if isinstance(exc, DisconnectError) \ 56 | or isinstance(exc, ConnectionResetError): 57 | print("Got disconnect / connection reset - shutting down.") 58 | sys.exit(0) 59 | raise context['exception'] 60 | 61 | try: 62 | loop = asyncio.get_event_loop() 63 | loop.set_exception_handler(handler) 64 | loop.run_until_complete(run()) 65 | except KeyboardInterrupt: 66 | print("Got CtrlC, shutting down.") 67 | 68 | 69 | if __name__ == '__main__': 70 | print("STARTING") 71 | main() 72 | print("FINISHED") 73 | -------------------------------------------------------------------------------- /experiments/bo_learner/run_get_analytics.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | import os 3 | 4 | python_interpreter = "/home/maarten/CLionProjects/revolve/venv/bin/python" 5 | path = "/home/maarten/CLionProjects/revolve/output/cpg_bo/main_1557477606/47/" 6 | 7 | 8 | paths = glob(path + "*/") 9 | print(len(paths)) 10 | paths = [p for p in paths if os.path.isfile(p + "fitnesses.txt")] 11 | print(len(paths)) 12 | 13 | for path in paths: 14 | # Call the experiment 15 | py_command = python_interpreter + " experiments/bo_learner/RunAnalysisBO.py " + path + "/ 50 1" 16 | os.system(py_command) -------------------------------------------------------------------------------- /experiments/examples/manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | This script loads a robot.yaml file and inserts it into the simulator. 4 | """ 5 | 6 | import os 7 | import sys 8 | import asyncio 9 | from pyrevolve.SDF.math import Vector3 10 | from pyrevolve import revolve_bot, parser 11 | from pyrevolve.tol.manage import World 12 | from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor 13 | from pyrevolve.evolution import fitness 14 | 15 | 16 | async def run(): 17 | """ 18 | The main coroutine, which is started below. 19 | """ 20 | robot_file_path = "experiments/examples/yaml/spider.yaml" 21 | 22 | # Parse command line / file input arguments 23 | settings = parser.parse_args() 24 | 25 | # Start Simulator 26 | if settings.simulator_cmd != 'debug': 27 | simulator_supervisor = DynamicSimSupervisor( 28 | world_file=settings.world, 29 | simulator_cmd=settings.simulator_cmd, 30 | simulator_args=["--verbose"], 31 | plugins_dir_path=os.path.join('.', 'build', 'lib'), 32 | models_dir_path=os.path.join('.', 'models'), 33 | simulator_name='gazebo' 34 | ) 35 | await simulator_supervisor.launch_simulator(port=settings.port_start) 36 | await asyncio.sleep(0.1) 37 | 38 | # Load a robot from yaml 39 | robot = revolve_bot.RevolveBot() 40 | robot.load_file(robot_file_path) 41 | robot.update_substrate() 42 | # robot._brain = BrainRLPowerSplines() 43 | 44 | # Connect to the simulator and pause 45 | connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) 46 | await asyncio.sleep(1) 47 | 48 | # Starts the simulation 49 | await connection.pause(False) 50 | 51 | # Insert the robot in the simulator 52 | robot_manager = await connection.insert_robot(robot, Vector3(0, 0, settings.z_start)) 53 | 54 | # Start a run loop to do some stuff 55 | while True: 56 | # Print robot fitness every second 57 | status = 'dead' if robot_manager.dead else 'alive' 58 | print(f"Robot fitness ({status}) is \n" 59 | f" OLD: {fitness.online_old_revolve(robot_manager)}\n" 60 | f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n" 61 | f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}") 62 | await asyncio.sleep(1.0) 63 | -------------------------------------------------------------------------------- /experiments/examples/only_gazebo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import os 4 | 5 | from pyrevolve import parser 6 | from pyrevolve.tol.manage import World 7 | from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor 8 | 9 | 10 | async def run(): 11 | # Parse command line / file input arguments 12 | settings = parser.parse_args() 13 | 14 | # Start Simulator 15 | if settings.simulator_cmd != 'debug': 16 | simulator_supervisor = DynamicSimSupervisor( 17 | world_file=settings.world, 18 | simulator_cmd=settings.simulator_cmd, 19 | simulator_args=["--verbose"], 20 | plugins_dir_path=os.path.join('.', 'build', 'lib'), 21 | models_dir_path=os.path.join('.', 'models'), 22 | simulator_name='gazebo' 23 | ) 24 | await simulator_supervisor.launch_simulator(port=settings.port_start) 25 | # let there be some time to sync all initial output of the simulator 26 | await asyncio.sleep(5) 27 | 28 | # Connect to the simulator and pause 29 | connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) 30 | await asyncio.sleep(1) 31 | await connection.disconnect() 32 | print("SIMULATOR STARTED") 33 | -------------------------------------------------------------------------------- /experiments/examples/run-experiments: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | while true 3 | do 4 | 5 | for i in {1..10}; do 6 | ./revolve.py --experiment-name default_experiment_$i --run $i --manager experiments/examples/manager_pop.py --n-cores 5; 7 | sleep 5s 8 | done 9 | done -------------------------------------------------------------------------------- /experiments/examples/sphere.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 0 0 0 0 0 4 | 5 | 0 0 .5 0 0 0 6 | 7 | 8 | 0.5 9 | 10 | 11 | 12 | 13 | 0.5 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /experiments/examples/stuck-experiments_watchman.py: -------------------------------------------------------------------------------- 1 | from datetime import datetime, timedelta 2 | import os 3 | 4 | # set these variables according to your experiments # 5 | dir_path = '../' 6 | experiments_names = ['default_experiment' 7 | ] 8 | runs = 1 9 | limit_of_minutes = 10 10 | # set these variables according to your experiments # 11 | 12 | some_has_been_updated = False 13 | 14 | while 1: 15 | 16 | # 10 minutes (do update this according to limit_of_minutes! 17 | os.system(" sleep 600s") 18 | 19 | youngest = [] 20 | for exp in experiments_names: 21 | for run in range(0, runs): 22 | 23 | path = dir_path + "/" + exp +'_'+str(run+1) + "/data_fullevolution/fitness" 24 | time_now = datetime.now() 25 | time_ago = time_now - timedelta(minutes=limit_of_minutes) 26 | 27 | if os.path.isdir(path): 28 | files = [] 29 | for r, d, f in os.walk(path): 30 | for file in f: 31 | filetime = datetime.fromtimestamp(os.path.getctime(path+'/'+file)) 32 | files.append(filetime) 33 | files.sort() 34 | youngest.append(files[-1]) 35 | 36 | if files[-1] > time_ago: 37 | some_has_been_updated = True 38 | 39 | if not some_has_been_updated: 40 | youngest.sort() 41 | print(str(time_now) + ': youngest file from ' + str(youngest[-1])) 42 | os.system(" kill $( ps aux | grep 'gzserver' | awk '{print $2}')") 43 | os.system(" kill $( ps aux | grep 'revolve.py' | awk '{print $2}')") 44 | print(' killled gzserver and revolve.py to force an error!') 45 | 46 | some_has_been_updated = False -------------------------------------------------------------------------------- /experiments/examples/tutorial1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import os 4 | from pyrevolve import parser 5 | from pyrevolve.gazebo.manage import WorldManager as World 6 | from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor 7 | from pyrevolve.custom_logging.logger import logger 8 | 9 | 10 | async def run(): 11 | logger.info('Hello World!') 12 | settings = parser.parse_args() 13 | 14 | # Start Simulator 15 | if settings.simulator_cmd != 'debug': 16 | simulator_supervisor = DynamicSimSupervisor( 17 | world_file=settings.world, 18 | simulator_cmd=settings.simulator_cmd, 19 | simulator_args=["--verbose"], 20 | plugins_dir_path=os.path.join('.', 'build', 'lib'), 21 | models_dir_path=os.path.join('.', 'models'), 22 | simulator_name='gazebo' 23 | ) 24 | await simulator_supervisor.launch_simulator(port=settings.port_start) 25 | await asyncio.sleep(0.1) 26 | 27 | connection = await World.create() 28 | if connection: 29 | logger.info("Connected to the simulator world.") 30 | 31 | await connection.pause(True) 32 | 33 | while True: 34 | await asyncio.sleep(10.0) 35 | -------------------------------------------------------------------------------- /experiments/examples/tutorial2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import os 4 | import sys 5 | 6 | current_dir = os.path.dirname(os.path.abspath(__file__)) 7 | rvpath = os.path.join(current_dir, '..', '..') 8 | sys.path.append(rvpath) 9 | 10 | from pygazebo.pygazebo import DisconnectError 11 | 12 | from pyrevolve.gazebo.manage import WorldManager as World 13 | from pyrevolve.sdfbuilder import Link 14 | from pyrevolve.sdfbuilder import Model 15 | from pyrevolve.sdfbuilder import SDF 16 | from pyrevolve.sdfbuilder.math import Vector3 17 | 18 | 19 | async def run(): 20 | world = await World.create() 21 | if world: 22 | print("Connected to the simulator world.") 23 | 24 | model = Model( 25 | name='sdf_model', 26 | static=True, 27 | ) 28 | model.set_position(position=Vector3(0, 0, 1)) 29 | link = Link('sdf_link') 30 | link.make_sphere( 31 | mass=10e10, 32 | radius=0.5, 33 | ) 34 | link.make_color(0.7, 0.2, 0.0, 1.0) 35 | 36 | model.add_element(link) 37 | sdf_model = SDF(elements=[model]) 38 | 39 | await world.insert_model(sdf_model) 40 | await world.pause(True) 41 | while True: 42 | await asyncio.sleep(10.0) 43 | 44 | 45 | def main(): 46 | def handler(loop, context): 47 | exc = context['exception'] 48 | if isinstance(exc, DisconnectError) \ 49 | or isinstance(exc, ConnectionResetError): 50 | print("Got disconnect / connection reset - shutting down.") 51 | sys.exit(0) 52 | raise context['exception'] 53 | 54 | try: 55 | loop = asyncio.get_event_loop() 56 | loop.set_exception_handler(handler) 57 | loop.run_until_complete(run()) 58 | except KeyboardInterrupt: 59 | print("Got Ctrl+C, shutting down.") 60 | 61 | 62 | if __name__ == "__main__": 63 | main() 64 | -------------------------------------------------------------------------------- /experiments/examples/tutorial3.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | This script loads a robot.yaml file and inserts it into the simulator. 4 | """ 5 | 6 | import os 7 | import sys 8 | import asyncio 9 | from pyrevolve.SDF.math import Vector3 10 | from pyrevolve import revolve_bot, parser 11 | from pyrevolve.tol.manage import World 12 | from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor 13 | from pyrevolve.evolution import fitness 14 | 15 | 16 | async def run(): 17 | """ 18 | The main coroutine, which is started below. 19 | """ 20 | robot_file_path = "experiments/examples/yaml/spider.yaml" 21 | 22 | # Parse command line / file input arguments 23 | settings = parser.parse_args() 24 | 25 | # Start Simulator 26 | if settings.simulator_cmd != 'debug': 27 | simulator_supervisor = DynamicSimSupervisor( 28 | world_file=settings.world, 29 | simulator_cmd=settings.simulator_cmd, 30 | simulator_args=["--verbose"], 31 | plugins_dir_path=os.path.join('.', 'build', 'lib'), 32 | models_dir_path=os.path.join('.', 'models'), 33 | simulator_name='gazebo' 34 | ) 35 | await simulator_supervisor.launch_simulator(port=settings.port_start) 36 | await asyncio.sleep(0.1) 37 | 38 | # Load a robot from yaml 39 | robot = revolve_bot.RevolveBot() 40 | robot.load_file(robot_file_path) 41 | robot.update_substrate() 42 | # robot._brain = BrainRLPowerSplines() 43 | 44 | # Connect to the simulator and pause 45 | connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) 46 | await asyncio.sleep(1) 47 | 48 | # Starts the simulation 49 | await connection.pause(False) 50 | 51 | # Insert the robot in the simulator 52 | robot_manager = await connection.insert_robot(robot, Vector3(0, 0, settings.z_start)) 53 | 54 | # Start a run loop to do some stuff 55 | while True: 56 | # Print robot fitness every second 57 | status = 'dead' if robot_manager.dead else 'alive' 58 | print(f"Robot fitness ({status}) is: {fitness.displacement(robot_manager, robot)} \n") 59 | await asyncio.sleep(1.0) 60 | 61 | -------------------------------------------------------------------------------- /experiments/examples/yaml/gecko7.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | body: 3 | id : Core 4 | type : Core 5 | children : 6 | 2: 7 | slot : 0 8 | id : Leg00Joint 9 | type : ActiveHinge 10 | orientation : 0 11 | children : 12 | 1: 13 | slot : 0 14 | id : Leg00 15 | type : FixedBrick 16 | orientation : 0 17 | 3: 18 | slot : 0 19 | id : Leg01Joint 20 | type : ActiveHinge 21 | orientation : 0 22 | children : 23 | 1: 24 | slot : 0 25 | id : Leg01 26 | type : FixedBrick 27 | orientation : 0 28 | 1: 29 | slot : 0 30 | id : BodyJoint0 31 | type : ActiveHinge 32 | orientation : 90 33 | children : 34 | 1: 35 | slot : 0 36 | id : Body0 37 | type : FixedBrick 38 | orientation : -90 39 | children : 40 | 1: 41 | slot : 0 42 | id : BodyJoint1 43 | type : ActiveHinge 44 | orientation : 90 45 | children : 46 | 1: 47 | slot : 0 48 | id : Body1 49 | type : FixedBrick 50 | orientation : -90 51 | children: 52 | 2: 53 | slot : 0 54 | id : Leg10Joint 55 | type : ActiveHinge 56 | orientation : 0 57 | children : 58 | 1: 59 | slot : 0 60 | id : Leg10 61 | type : FixedBrick 62 | orientation : 0 63 | 3: 64 | slot : 0 65 | id : Leg11Joint 66 | type : ActiveHinge 67 | orientation : 0 68 | children : 69 | 1: 70 | slot : 0 71 | id : Leg11 72 | type : FixedBrick 73 | orientation : 0 -------------------------------------------------------------------------------- /experiments/examples/yaml/snake.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | id: example_snake 3 | body: 4 | id : Core 5 | type : Core 6 | params: 7 | red: 0.94 8 | green: 0.98 9 | blue: 0.05 10 | children : 11 | 0: 12 | id : Leg00Joint 13 | type : ActiveHinge 14 | params: 15 | red: 0.98 16 | green: 0.98 17 | blue: 0.98 18 | orientation : 90 19 | children : 20 | 1: 21 | id : Leg00 22 | type : FixedBrick 23 | params: 24 | red: 0.94 25 | green: 0.98 26 | blue: 0.05 27 | orientation : -90 28 | children : 29 | 1: 30 | id : Leg01Joint 31 | type : ActiveHinge 32 | params: 33 | red: 0.98 34 | green: 0.98 35 | blue: 0.98 36 | children : 37 | 1: 38 | id : Leg01 39 | type : FixedBrick 40 | params: 41 | red: 0.94 42 | green: 0.98 43 | blue: 0.05 44 | orientation : 0 45 | 1: 46 | id : Leg10Joint 47 | type : ActiveHinge 48 | params: 49 | red: 0.98 50 | green: 0.98 51 | blue: 0.98 52 | orientation : 90 53 | children : 54 | 1: 55 | id : Leg10 56 | type : FixedBrick 57 | params: 58 | red: 0.94 59 | green: 0.98 60 | blue: 0.05 61 | orientation : -90 62 | children : 63 | 1: 64 | id : Leg11Joint 65 | type : ActiveHinge 66 | params: 67 | red: 0.98 68 | green: 0.98 69 | blue: 0.98 70 | children : 71 | 1: 72 | id : Leg11 73 | type : FixedBrick 74 | params: 75 | red: 0.94 76 | green: 0.98 77 | blue: 0.05 78 | orientation : 0 79 | brain: 80 | type: rlpower-splines -------------------------------------------------------------------------------- /experiments/karines_experiments/run-experiments: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | while true 3 | do 4 | for i in {1..10}; do 5 | ./revolve.py --experiment_name karines_experiments/data/lava_$i --run $i --manager experiments/karines_experiments/manager_pop_lava.py --n-cores 5; 6 | sleep 5s 7 | ./revolve.py --experiment_name karines_experiments/data/plane_$i --run $i --manager experiments/karines_experiments/manager_pop.py --n-cores 5; 8 | sleep 5s 9 | done 10 | done -------------------------------------------------------------------------------- /experiments/karines_experiments/stuck-experiments_watchman.py: -------------------------------------------------------------------------------- 1 | from datetime import datetime, timedelta 2 | import os 3 | 4 | # set these variables according to your experiments # 5 | dir_path = 'data' 6 | experiments_names = ['plane', 7 | 'lava' 8 | ] 9 | runs = 10 10 | limit_of_minutes = 10 11 | # set these variables according to your experiments # 12 | 13 | some_has_been_updated = False 14 | 15 | while 1: 16 | 17 | # 10 minutes (do update this according to limit_of_minutes! 18 | os.system(" sleep 600s") 19 | 20 | youngest = [] 21 | for exp in experiments_names: 22 | for run in range(0, runs): 23 | 24 | path = os.path.join(dir_path, exp, str(run+1), 'data_fullevolution', 'fitness') 25 | time_now = datetime.now() 26 | time_ago = time_now - timedelta(minutes=limit_of_minutes) 27 | 28 | if os.path.isdir(path): 29 | files = [] 30 | for r, d, f in os.walk(path): 31 | for file in f: 32 | filetime = datetime.fromtimestamp(os.path.getctime(path+'/'+file)) 33 | files.append(filetime) 34 | files.sort() 35 | youngest.append(files[-1]) 36 | 37 | if files[-1] > time_ago: 38 | some_has_been_updated = True 39 | 40 | if not some_has_been_updated: 41 | youngest.sort() 42 | print(str(time_now) + ': youngest file from ' + str(youngest[-1])) 43 | os.system(" kill $( ps aux | grep 'gzserver' | awk '{print $2}')") 44 | os.system(" kill $( ps aux | grep 'revolve.py' | awk '{print $2}')") 45 | print(' killled gzserver and revolve.py to force an error!') 46 | 47 | some_has_been_updated = False 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /experiments/unmanaged/.gitignore: -------------------------------------------------------------------------------- 1 | /[0-9]*/ -------------------------------------------------------------------------------- /models/rg_robot/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | RoboGen robot 4 | 1.0 5 | robot.sdf 6 | 7 | 8 | Elte Hupkes 9 | elte@hupkes.org 10 | 11 | 12 | 13 | An example of a RoboGen modular robot in X shape. 14 | 15 | 16 | -------------------------------------------------------------------------------- /models/thymio/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Thymio 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Milan Jelisavcic 9 | m.j.jelisavcic@vu.nl 10 | 11 | 12 | 13 | My awesome robot. 14 | 15 | 16 | -------------------------------------------------------------------------------- /models/tilted10/_DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tilted10/_DS_Store -------------------------------------------------------------------------------- /models/tilted10/meshes/_DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tilted10/meshes/_DS_Store -------------------------------------------------------------------------------- /models/tilted10/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Ground Plane 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A simple ground plane. 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/tilted15/_DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tilted15/_DS_Store -------------------------------------------------------------------------------- /models/tilted15/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Ground Plane 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A simple ground plane. 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/tilted5/_DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tilted5/_DS_Store -------------------------------------------------------------------------------- /models/tilted5/meshes/_DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tilted5/meshes/_DS_Store -------------------------------------------------------------------------------- /models/tilted5/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Ground Plane 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A simple ground plane. 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/tol_arena/arena.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 0.0 0.0 0.0 0.0 0.0 0.0 6 | 7 | 0.0 0.0 1.0 0.0 0.0 0.0 8 | 9 | 1.0 10 | 11 | 0.083 12 | 0.0 13 | 0.0 14 | 0.083 15 | 0.0 16 | 0.083 17 | 18 | 19 | 20 | 21 | 22 | model://tol_arena/meshes/BirthClinic.dae 23 | 24 | 25 | 26 | 27 | 28 | 29 | model://tol_arena/meshes/BirthClinic.dae 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /models/tol_arena/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Triangle of Life arena 4 | 1.0 5 | arena.sdf 6 | 7 | 8 | Elte Hupkes 9 | elte@hupkes.org 10 | 11 | 12 | 13 | An example of Triangle of Life arena. 14 | 15 | 16 | -------------------------------------------------------------------------------- /models/tol_ground/materials/scripts/floor.material: -------------------------------------------------------------------------------- 1 | material Floor/Wood1 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | 8 | texture_unit 9 | { 10 | texture wood1.png 11 | scale 0.01 0.01 12 | } 13 | } 14 | } 15 | } 16 | 17 | material Floor/Wood2 18 | { 19 | technique 20 | { 21 | pass 22 | { 23 | 24 | texture_unit 25 | { 26 | texture wood2.png 27 | scale 0.02 0.02 28 | } 29 | } 30 | } 31 | } 32 | 33 | material Floor/Wood3 34 | { 35 | technique 36 | { 37 | pass 38 | { 39 | 40 | texture_unit 41 | { 42 | texture wood3.png 43 | scale 0.05 0.05 44 | } 45 | } 46 | } 47 | } 48 | 49 | material Floor/Concrete1 50 | { 51 | technique 52 | { 53 | pass 54 | { 55 | 56 | texture_unit 57 | { 58 | texture concrete1.png 59 | scale 0.04 0.04 60 | } 61 | } 62 | } 63 | } 64 | 65 | material Floor/Grass 66 | { 67 | technique 68 | { 69 | pass 70 | { 71 | 72 | texture_unit 73 | { 74 | texture grass.png 75 | scale 0.01 0.01 76 | } 77 | } 78 | } 79 | } 80 | 81 | material Floor/Asphalt 82 | { 83 | technique 84 | { 85 | pass 86 | { 87 | 88 | texture_unit 89 | { 90 | texture asphalt.png 91 | scale 0.01 0.01 92 | } 93 | } 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /models/tol_ground/materials/textures/asphalt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tol_ground/materials/textures/asphalt.png -------------------------------------------------------------------------------- /models/tol_ground/materials/textures/concrete1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tol_ground/materials/textures/concrete1.png -------------------------------------------------------------------------------- /models/tol_ground/materials/textures/grass.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tol_ground/materials/textures/grass.png -------------------------------------------------------------------------------- /models/tol_ground/materials/textures/wood1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tol_ground/materials/textures/wood1.png -------------------------------------------------------------------------------- /models/tol_ground/materials/textures/wood2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tol_ground/materials/textures/wood2.png -------------------------------------------------------------------------------- /models/tol_ground/materials/textures/wood3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/models/tol_ground/materials/textures/wood3.png -------------------------------------------------------------------------------- /models/tol_ground/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Ground Plane 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A simple ground plane. 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/tol_ground/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | true 6 | 7 | 8 | 9 | 10 | 0 0 1 11 | 100 100 12 | 13 | 14 | 15 | 16 | 17 | 18 | 1 19 | 1 20 | 0.01 21 | 0.01 22 | 23 | 24 | 25 | 26 | 9.000000e+04 27 | 2.000000e+06 28 | 29 | 30 | 31 | 32 | 33 | false 34 | 35 | 36 | 0 0 1 37 | 100 100 38 | 39 | 40 | 41 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /pyrevolve/SDF/__init__.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree 2 | 3 | from .pose import Pose, Posable 4 | from .link import Link 5 | from .geometry import Visual, Collision, MeshGeometry, BoxGeometry 6 | from .inertial import Inertial 7 | from .joint import Joint 8 | from .sensor import CameraSensor, TouchSensor, IMUSensor 9 | from . import math 10 | from .revolve_bot_sdf_builder import revolve_bot_to_sdf 11 | 12 | 13 | def sub_element_text(parent, name, text): 14 | el = xml.etree.ElementTree.SubElement(parent, name) 15 | if type(text) is float or type(text) is int: 16 | el.text = '{:e}'.format(text) 17 | else: 18 | el.text = str(text) 19 | return el 20 | -------------------------------------------------------------------------------- /pyrevolve/SDF/math/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Vector, Quaternion and RotationMatrix classes written as 3 | wrappers over `transformations.py` (see that file for license/origin). 4 | """ 5 | from .classes import Vector3, Quaternion, RotationMatrix 6 | -------------------------------------------------------------------------------- /pyrevolve/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .config import parser 4 | from .config import str_to_address 5 | from .config import make_revolve_config -------------------------------------------------------------------------------- /pyrevolve/angle/README.md: -------------------------------------------------------------------------------- 1 | # Revolve.Angle 2 | Revolve.Angle is the more opinionated part of revolve that can be used to quickly 3 | scaffold a robot experiment. Hereto it specifies a full robot genome with 4 | the following properties: 5 | 6 | - The genome is a tree, with a root node. No cycles (like the Revolve body space). 7 | - Each node of the tree corresponds to a body part. Additionally, each node 8 | has a number of hidden neurons and neural connections. 9 | - Neural connections do not point to specific nodes - rather they encode a 10 | path, type and offset. Following the path through the tree leads to 11 | a different node, where the type and offset specify a neuron. When 12 | evolving the robot this might thus point connections to different 13 | locations, or connections might no longer exist at all. 14 | 15 | Using Revolve.Angle, robot trees can be generated and evolved using crossover 16 | and mutation. Each robot tree can be converted to a Protobuf Robot object 17 | (see `cpp/msgs/robot.proto`), which can then be turned into SDF and used 18 | for simulation. 19 | 20 | Revolve.Angle is opinionated in that it forces the use of a neural network 21 | in combination with the Revolve body tree structure. The user is still 22 | free to choose what types of body parts and neurons are used. 23 | 24 | Revolve.Angle is still very much a work in progress. 25 | -------------------------------------------------------------------------------- /pyrevolve/angle/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | See README.md for information on Revolve.Angle 3 | """ 4 | from __future__ import absolute_import 5 | 6 | from .generate import TreeGenerator 7 | from .evolve import Crossover, Mutator 8 | from .representation import Tree, Node 9 | from .manage import WorldManager, RobotManager 10 | __author__ = 'Elte Hupkes' 11 | -------------------------------------------------------------------------------- /pyrevolve/angle/generate.py: -------------------------------------------------------------------------------- 1 | """ 2 | Generates a robot tree with the help of Revolve.Generate. This is a bit 3 | backwards, but it allows for more configurability. A generator is defined 4 | with a body generator and a neural net generator. First, a body is 5 | generated. The interface of this body is then fed to the neural net 6 | generator resulting in a neural network. 7 | """ 8 | from __future__ import absolute_import 9 | 10 | from .representation import Tree 11 | 12 | 13 | class TreeGenerator(object): 14 | def __init__(self, body_gen, brain_gen): 15 | """ 16 | :param body_gen: 17 | :type body_gen: BodyGenerator 18 | :param brain_gen: Neural network generator. Note that this generator 19 | should set the partId property of input / output 20 | neurons. 21 | :type brain_gen: NeuralNetworkGenerator 22 | :return: 23 | """ 24 | self.body_gen = body_gen 25 | self.brain_gen = brain_gen 26 | 27 | def generate_tree(self): 28 | """ 29 | Generates a new robot tree 30 | 31 | :return: 32 | :rtype: Tree 33 | """ 34 | # Generate a body 35 | body = self.body_gen.generate() 36 | 37 | # Generate a brain from this body 38 | brain = self.brain_gen.generate_from_body(body, self.body_gen.spec) 39 | 40 | return Tree.from_body_brain(body, brain, self.body_gen.spec) 41 | -------------------------------------------------------------------------------- /pyrevolve/angle/manage/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .robotmanager import RobotManager 4 | from .world import WorldManager 5 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/README.md: -------------------------------------------------------------------------------- 1 | # Robogen body parts 2 | This package contains component code for many of the Robogen body parts 3 | available at the time of this writing. -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .config import Config 4 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/body_parts/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .active_hinge import ActiveHinge 4 | from .core_component import CoreComponent 5 | from .hinge import Hinge 6 | from .light_sensor import LightSensor 7 | from .touch_sensor import TouchSensor 8 | from .fixed_brick import FixedBrick 9 | from .parametric_bar_joint import ParametricBarJoint 10 | from .wheel import * 11 | from .cardan import * 12 | from .active_cardan import * 13 | from .active_rotator import * 14 | from .active_wheel import * 15 | from .active_wheg import * 16 | 17 | __author__ = 'Elte Hupkes' 18 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/body_parts/fixed_brick.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | 4 | # Revolve / sdfbuilder imports 5 | from ....build.sdf import Box 6 | from ....build.util import in_grams, in_mm 7 | from pyrevolve.sdfbuilder.structure import Box as BoxGeom, Mesh 8 | from pyrevolve.sdfbuilder.math import Vector3 9 | 10 | # Local imports 11 | from .util import ColorMixin 12 | 13 | WIDTH = in_mm(41) 14 | HEIGHT = in_mm(35.5) 15 | MASS = in_grams(10.2) 16 | 17 | 18 | class FixedBrick(Box, ColorMixin): 19 | """ 20 | Brick - same size as the core component, but 21 | without any sensors. We can conveniently model this 22 | as a box. 23 | """ 24 | X = WIDTH 25 | Y = WIDTH 26 | Z = HEIGHT 27 | MASS = MASS 28 | 29 | def _initialize(self, **kwargs): 30 | self.component = self.create_component( 31 | BoxGeom(self.x, self.y, self.z, self.mass), "box", 32 | visual=Mesh("model://rg_robot/meshes/FixedBrick.dae")) 33 | self.apply_color() 34 | 35 | def get_slot(self, slot): 36 | """ 37 | There's only one slot, return the link. 38 | """ 39 | self.check_slot(slot) 40 | return self.component 41 | 42 | def get_slot_position(self, slot): 43 | """ 44 | Return slot position 45 | """ 46 | self.check_slot(slot) 47 | vmax = WIDTH / 2.0 48 | if slot == 0: 49 | # Front face 50 | return Vector3(0, -vmax, 0) 51 | elif slot == 1: 52 | # Back face 53 | return Vector3(0, vmax, 0) 54 | elif slot == 2: 55 | # Right face 56 | return Vector3(vmax, 0, 0) 57 | 58 | # Left face 59 | return Vector3(-vmax, 0, 0) 60 | 61 | def get_slot_normal(self, slot): 62 | """ 63 | Return slot normal. 64 | """ 65 | return self.get_slot_position(slot).normalized() 66 | 67 | def get_slot_tangent(self, slot): 68 | """ 69 | Return slot tangent 70 | """ 71 | self.check_slot(slot) 72 | 73 | # The top face is tangent to all supported 74 | # slots in the plane, front, back, right, left 75 | # Since there's no particular reason for choosing any 76 | # other we just return the top face for all. 77 | return Vector3(0, 0, 1) 78 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/body_parts/light_sensor.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import print_function 3 | 4 | # Revolve imports 5 | from ....build.sdf import Box 6 | from ....build.util import in_grams, in_mm 7 | 8 | # SDF builder imports 9 | from pyrevolve.sdfbuilder.math import Vector3 10 | from pyrevolve.sdfbuilder.sensor import Sensor as SdfSensor 11 | from pyrevolve.sdfbuilder.util import number_format as nf 12 | 13 | # Local imports 14 | from .util import ColorMixin 15 | 16 | MASS = in_grams(2) 17 | SENSOR_BASE_WIDTH = in_mm(34) 18 | SENSOR_BASE_THICKNESS = in_mm(1.5) 19 | 20 | 21 | class LightSensor(Box, ColorMixin): 22 | """ 23 | Simple light sensor. This extends `Box` for convenience, 24 | make sure you set the arity to 1 in the body specification. 25 | """ 26 | 27 | def _initialize(self, **kwargs): 28 | """ 29 | :param kwargs: 30 | :return: 31 | """ 32 | self.mass = MASS 33 | self.x = SENSOR_BASE_THICKNESS 34 | self.y = self.z = SENSOR_BASE_WIDTH 35 | super(LightSensor, self)._initialize(**kwargs) 36 | 37 | # Add the SDF camera sensor 38 | camera = SdfSensor( 39 | name="{}_light_sensor".format(self.id), 40 | sensor_type="camera", 41 | update_rate=self.conf.sensor_update_rate) 42 | 43 | # TODO: Set field of view 44 | cam_details = "" \ 45 | "" \ 46 | "11" \ 47 | "" \ 48 | "{}{}" \ 49 | "".format(nf(in_mm(1)), nf(in_mm(50000))) 50 | camera.add_element(cam_details) 51 | camera.set_position(Vector3(0.5 * self.x, 0, 0)) 52 | self.component.add_sensor(camera, "light") 53 | 54 | if self.conf.visualize_sensors: 55 | camera.add_element("1") 56 | 57 | self.apply_color() 58 | 59 | def get_slot_position(self, slot): 60 | self.check_slot(slot) 61 | return Vector3(-0.5 * SENSOR_BASE_THICKNESS, 0, 0) 62 | 63 | def get_slot_tangent(self, slot): 64 | self.check_slot(slot) 65 | return Vector3(0, 1, 0) 66 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/body_parts/util.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | 4 | class ColorMixin(object): 5 | """ 6 | Mixin class for "colorable" parts. Needs to be mixed 7 | in with a body part, or it won't work. 8 | """ 9 | 10 | def apply_color(self): 11 | """ 12 | Applies the "red", "green" and "blue" arguments 13 | to all links in this body part. 14 | """ 15 | red = self.part_params.get("red", 0.5) 16 | green = self.part_params.get("green", 0.5) 17 | blue = self.part_params.get("blue", 0.5) 18 | self.make_color(red, green, blue) 19 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/body_parts/wheel.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | # Revolve imports 4 | from ....build.sdf import BodyPart, ComponentJoint as Joint 5 | from ....build.util import in_grams, in_mm 6 | 7 | from pyrevolve.sdfbuilder.joint import Limit 8 | from pyrevolve.sdfbuilder.math import Vector3 9 | from pyrevolve.sdfbuilder.structure import Cylinder, Box 10 | 11 | # Local imports 12 | from .util import ColorMixin 13 | from .. import constants 14 | 15 | MASS_SLOT = in_grams(2.5) 16 | MASS_WHEEL = in_grams(4) 17 | 18 | SLOT_WIDTH = in_mm(34) 19 | SLOT_THICKNESS = in_mm(10.75) 20 | SLOT_CONNECTION_THICKNESS = in_mm(1.5) 21 | SLOT_WHEEL_OFFSET = in_mm(7.5) 22 | WHEEL_THICKNESS = in_mm(3) 23 | 24 | 25 | class Wheel(BodyPart, ColorMixin): 26 | """ 27 | Passive wheel 28 | """ 29 | 30 | def _initialize(self, **kwargs): 31 | self.radius = in_mm(kwargs['radius']) 32 | 33 | wheel = self.create_component(Cylinder(self.radius, WHEEL_THICKNESS, MASS_WHEEL), "wheel") 34 | self.root = self.create_component(Box(SLOT_WIDTH, SLOT_WIDTH, SLOT_THICKNESS, MASS_SLOT)) 35 | 36 | # Create the wheel 37 | z_wheel = 0.5 * SLOT_THICKNESS - (SLOT_THICKNESS + SLOT_CONNECTION_THICKNESS - SLOT_WHEEL_OFFSET) 38 | wheel.set_position(Vector3(0, 0, z_wheel)) 39 | 40 | # Attach the wheel and the root with a revolute joint 41 | joint = Joint("revolute", self.root, wheel, axis=Vector3(0, 0, -1)) 42 | 43 | # TODO Adequate force limit for passive wheel 44 | joint.axis.limit = Limit(effort=constants.MAX_SERVO_TORQUE_ROTATIONAL) 45 | self.add_joint(joint) 46 | 47 | # Call color mixin 48 | self.apply_color() 49 | 50 | def get_slot(self, slot_id): 51 | self.check_slot(slot_id) 52 | return self.root 53 | 54 | def get_slot_position(self, slot_id): 55 | return Vector3(0, 0, -0.5 * SLOT_THICKNESS) 56 | 57 | def get_slot_normal(self, slot_id): 58 | return Vector3(0, 0, -1) 59 | 60 | def get_slot_tangent(self, slot_id): 61 | return Vector3(0, 1, 0) 62 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/config.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | 4 | class Config(object): 5 | def __init__(self, 6 | min_parts, 7 | max_parts, 8 | max_inputs, 9 | max_outputs, 10 | body_mutation_epsilon=0.05, 11 | enforce_planarity=True, 12 | disable_sensors=False, 13 | enable_touch_sensor=True, 14 | enable_light_sensor=False, 15 | enable_wheel_parts=False): 16 | self.max_inputs = max_inputs 17 | self.max_outputs = max_outputs 18 | self.body_mutation_epsilon = body_mutation_epsilon 19 | self.enforce_planarity = enforce_planarity 20 | self.disable_sensors = disable_sensors 21 | self.enable_touch_sensor = enable_touch_sensor 22 | self.enable_light_sensor = enable_light_sensor 23 | self.enable_wheel_parts = enable_wheel_parts 24 | self.max_parts = max_parts 25 | self.min_parts = min_parts 26 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/constants.py: -------------------------------------------------------------------------------- 1 | """ 2 | ToL constants 3 | """ 4 | from __future__ import absolute_import 5 | from __future__ import division 6 | 7 | import math 8 | 9 | from ...build.sdf import PID 10 | 11 | MAX_SERVO_TORQUE = 1.8 * 9.81 / 100 12 | """ Expressed in Newton*m from kg-cm = ((kg-cm)*g)/100 """ 13 | 14 | MAX_SERVO_TORQUE_ROTATIONAL = 4 * 9.81 / 100 15 | """ Expressed in Newton*m from kg-cm = ((kg-cm)*g)/100 """ 16 | 17 | MAX_SERVO_VELOCITY = (50.0/60.0) * 2 * math.pi 18 | """ Maximum rotational velocity of a servo, in radians / second """ 19 | 20 | SERVO_LIMIT = math.radians(45) 21 | """ Upper and lower limit """ 22 | 23 | HINGE_LIMIT = math.radians(60) 24 | """ Upper and lower limit for hinge """ 25 | 26 | CARDAN_LIMIT = math.radians(60) 27 | """ Upper and lower limit of each axis of rotation """ 28 | 29 | SERVO_VELOCITY_PID = PID( 30 | proportional_gain=0.5, 31 | derivative_gain=0.0, 32 | integral_gain=0, 33 | integral_max=0 34 | ) 35 | """ Default servo velocity PID. **Currently unused** as velocity is abstractly set 36 | on the servo. """ 37 | 38 | SERVO_POSITION_PID = PID( 39 | proportional_gain=0.9, 40 | derivative_gain=0, 41 | integral_gain=0, 42 | integral_max=0 43 | ) 44 | """ Default servo position PID. """ 45 | 46 | # Original parameter values from RobogenCollision.cpp 47 | SURFACE_FRICTION1 = 1.0 48 | SURFACE_FRICTION2 = 1.0 49 | SURFACE_SLIP1 = 0.01 50 | SURFACE_SLIP2 = 0.01 51 | 52 | # soft_erp / soft_cfm for surfaces cannot be set directly 53 | # in Gazebo, so they are calculated into kp and kd here using 54 | # the step size and the intended values. Make sure to change 55 | # them when you change simulation parameters (and change 56 | # them in `tol_ground` as well). 57 | SURFACE_ERP = 0.1 58 | SURFACE_CFM = 10e-6 59 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/spec/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .robot import RobogenTreeGenerator, make_planar 4 | from .body import BodyGenerator, get_body_spec 5 | 6 | __author__ = 'Elte Hupkes' 7 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/spec/robot.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from ... import TreeGenerator 4 | 5 | 6 | class RobogenTreeGenerator(TreeGenerator): 7 | """ 8 | Tree generator with some additions. 9 | """ 10 | 11 | def __init__(self, body_gen, brain_gen, conf): 12 | """ 13 | :return: 14 | """ 15 | self.conf = conf 16 | super(RobogenTreeGenerator, self).__init__(body_gen, brain_gen) 17 | 18 | def generate_tree(self): 19 | """ 20 | Overrides `generate_tree` to force robot planarity. Robots 21 | without output neurons are also discarded because we can be 22 | certain they will not be able to move. 23 | :return: 24 | """ 25 | outputs = -1 26 | tree = None 27 | while outputs <= 0: 28 | tree = super(RobogenTreeGenerator, self).generate_tree() 29 | _, outputs, _ = tree.root.io_count(recursive=True) 30 | 31 | if self.conf.enforce_planarity: 32 | make_planar(tree.root) 33 | 34 | return tree 35 | 36 | 37 | def make_planar(node, current_orientation=0): 38 | """ 39 | Takes a generated tree and enforces planarity. This uses 40 | the convenient fact that all current body part slots 41 | lie in a plane, so we only have to fix it so that 42 | bricks and parametric bar joints have an overall zero 43 | orientation. 44 | 45 | :param node: 46 | :type node: Node 47 | :param current_orientation: Method is called recursively, using this 48 | as the traversing orientation. 49 | :return: 50 | """ 51 | if node.is_root(): 52 | node.part.orientation = 0 53 | elif node.part.type in ("FixedBrick", "ParametricBarJoint"): 54 | node.part.orientation = (360 - current_orientation) % 360 55 | 56 | for conn in node.child_connections(): 57 | make_planar( 58 | conn.node, 59 | (current_orientation + node.part.orientation) % 360) 60 | -------------------------------------------------------------------------------- /pyrevolve/angle/robogen/util.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | 4 | from .constants import * 5 | 6 | from pyrevolve.sdfbuilder.physics import Friction 7 | from pyrevolve.sdfbuilder.structure import Collision 8 | from pyrevolve.sdfbuilder.util import number_format as nf 9 | from pyrevolve.sdfbuilder import Element 10 | 11 | 12 | def apply_surface_parameters(model, intended_step_size=0.005): 13 | """ 14 | Applies surface parameters to all collisions in the given model. 15 | :param model: 16 | :type model: Model 17 | :param intended_step_size: 18 | :return: 19 | """ 20 | surface_kd = SURFACE_ERP / (SURFACE_CFM * intended_step_size) 21 | surface_kp = 1.0 / SURFACE_CFM - (SURFACE_ERP / SURFACE_CFM) 22 | 23 | # Add friction surfaces to all body parts 24 | surf = Element(tag_name="surface") 25 | friction = Friction( 26 | friction=SURFACE_FRICTION1, 27 | friction2=SURFACE_FRICTION2, 28 | slip1=SURFACE_SLIP1, 29 | slip2=SURFACE_SLIP2 30 | ) 31 | contact = "" \ 32 | "" \ 33 | "{}" \ 34 | "{}" \ 35 | "" \ 36 | "".format( 37 | nf(surface_kd), nf(surface_kp) 38 | ) 39 | 40 | surf.add_element(contact) 41 | surf.add_element(friction) 42 | 43 | collisions = model.get_elements_of_type(Collision, recursive=True) 44 | for collision in collisions: 45 | collision.add_element(surf) 46 | -------------------------------------------------------------------------------- /pyrevolve/build/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/build/__init__.py -------------------------------------------------------------------------------- /pyrevolve/build/sdf/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .battery import BasicBattery 4 | 5 | from .body import BodyPart 6 | from .body import Box 7 | from .body import Cylinder 8 | from .body import ComponentJoint 9 | 10 | from .builder import AspectBuilder 11 | from .builder import RobotBuilder 12 | from .builder import BodyBuilder 13 | from .builder import NeuralNetBuilder 14 | 15 | from .motor import Motor 16 | from .motor import PID 17 | from .motor import PIDMotor 18 | from .motor import VelocityMotor 19 | from .motor import PositionMotor 20 | 21 | from .neural_net import Neuron 22 | from .neural_net import NeuralConnection 23 | 24 | from .sensor import Sensor 25 | from .sensor import VirtualSensor 26 | from .sensor import BasicBatterySensor 27 | from .sensor import PointIntensitySensor 28 | 29 | -------------------------------------------------------------------------------- /pyrevolve/build/sdf/battery.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from pyrevolve.sdfbuilder import Element 4 | from pyrevolve.sdfbuilder.util import number_format as nf 5 | 6 | 7 | class BasicBattery(Element): 8 | """ 9 | The rv:battery element, to be included in a robot's plugin 10 | """ 11 | TAG_NAME = 'rv:battery' 12 | 13 | def __init__(self, level): 14 | """ 15 | 16 | :param level: Initial battery level 17 | :type level: float 18 | :return: 19 | """ 20 | super(BasicBattery, self).__init__() 21 | self.level = level 22 | 23 | def render_elements(self): 24 | """ 25 | 26 | :return: 27 | """ 28 | elms = super(BasicBattery, self).render_elements() 29 | return elms + [Element(tag_name="rv:level", body=nf(self.level))] 30 | -------------------------------------------------------------------------------- /pyrevolve/build/sdf/body/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .body_part import BodyPart 4 | from .box import Box 5 | from .cylinder import Cylinder 6 | from .component import Component 7 | from .joint import ComponentJoint 8 | -------------------------------------------------------------------------------- /pyrevolve/build/sdf/body/cylinder.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | 4 | from pyrevolve.sdfbuilder.math import Vector3 5 | from pyrevolve.sdfbuilder.structure import Cylinder as CylinderGeom 6 | 7 | from .body_part import BodyPart 8 | 9 | 10 | class Cylinder(BodyPart): 11 | """ 12 | A base class that allows you to quicky define a body 13 | part that is just a simple cylinder. 14 | 15 | By default a cylinder has two slots: one on the center of each 16 | flat side. 17 | """ 18 | # Override these in child class for basic functionality 19 | MASS = 1.0 20 | """The mass of the box""" 21 | 22 | RADIUS = 1.0 23 | """ The radius of the cylinder (in x and y directions)""" 24 | 25 | LENGTH = 1.0 26 | """ The length of the cylinder (in z direction) """ 27 | 28 | def __init__(self, part_id, conf, **kwargs): 29 | """ 30 | """ 31 | # Set class properties here, if desired they can 32 | # still be overwritten in _initialize. 33 | self.mass, self.radius, self.length = self.MASS, self.RADIUS, self.LENGTH 34 | super(Cylinder, self).__init__(part_id, conf, **kwargs) 35 | 36 | def _initialize(self, **kwargs): 37 | """ 38 | :param kwargs: 39 | :return: 40 | """ 41 | self.component = self.create_component(CylinderGeom( 42 | self.radius, self.length, self.mass), "cylinder") 43 | 44 | def get_slot(self, slot_id): 45 | """ 46 | :param slot_id: 47 | :return: 48 | """ 49 | self.check_slot(slot_id) 50 | return self.component 51 | 52 | def get_slot_position(self, slot_id): 53 | """ 54 | Returns the slot position for the given slot. Slot 0 is 55 | defined as the top, slot 1 is the bottom. 56 | :param slot_id: 57 | :return: 58 | """ 59 | z = self.length / 2.0 60 | return Vector3(0, 0, z if slot_id == 0 else -z) 61 | 62 | def get_slot_normal(self, slot_id): 63 | """ 64 | :param slot_id: 65 | :return: 66 | """ 67 | return self.get_slot_position(slot_id).normalized() 68 | 69 | def get_slot_tangent(self, slot_id): 70 | """ 71 | We use the same tangent vector for every slot, pointing 72 | horizontally. 73 | :param slot_id: 74 | :return: 75 | """ 76 | return Vector3(1, 0, 0) 77 | 78 | -------------------------------------------------------------------------------- /pyrevolve/build/sdf/body/exception.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | 4 | class ArityException(BaseException): 5 | def __init__(self, *args, **kwargs): 6 | super(ArityException, self).__init__(self, *args, **kwargs) 7 | 8 | 9 | class ComponentException(BaseException): 10 | def __init__(self, *args, **kwargs): 11 | super(ComponentException, self).__init__(self, *args, **kwargs) 12 | -------------------------------------------------------------------------------- /pyrevolve/build/sdf/neural_net/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .neuron import * 4 | -------------------------------------------------------------------------------- /pyrevolve/build/sdf/neural_net/neuron.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from pyrevolve.sdfbuilder import Element 4 | from pyrevolve.sdfbuilder.util import number_format as nf 5 | 6 | from ....spec import Neuron as ProtoNeuron 7 | from ....spec import NeuralConnection as ProtoNeuralConnection 8 | 9 | 10 | class Neuron(Element): 11 | """ 12 | Plugin neuron element. 13 | """ 14 | TAG_NAME = 'rv:neuron' 15 | 16 | def __init__(self, neuron, params): 17 | """ 18 | :param neuron: Protobuf neuron 19 | :type neuron: ProtoNeuron 20 | :param params: 21 | :return: 22 | """ 23 | super(Neuron, self).__init__() 24 | self.neuron = neuron 25 | self.params = params 26 | 27 | def render_attributes(self): 28 | """ 29 | Adds type / ID / layer attributes 30 | """ 31 | attrs = super(Neuron, self).render_attributes() 32 | attrs.update({ 33 | 'layer': self.neuron.layer, 34 | 'type': self.neuron.type, 35 | 'id': self.neuron.id 36 | }) 37 | 38 | if self.neuron.HasField("partId"): 39 | attrs['part_id'] = self.neuron.partId 40 | 41 | return attrs 42 | 43 | def render_elements(self): 44 | """ 45 | Adds attributes as elements 46 | """ 47 | elms = [Element( 48 | tag_name='rv:'+param, 49 | body=nf(self.params[param])) for param in self.params] 50 | return super(Neuron, self).render_elements() + elms 51 | 52 | 53 | class NeuralConnection(Element): 54 | """ 55 | Plugin neural connection element. 56 | """ 57 | TAG_NAME = 'rv:neural_connection' 58 | 59 | def __init__(self, conn): 60 | """ 61 | :param conn: 62 | :type conn: ProtoNeuralConnection 63 | :return: 64 | """ 65 | super(NeuralConnection, self).__init__() 66 | self.conn = conn 67 | 68 | def render_attributes(self): 69 | """ 70 | """ 71 | attrs = super(NeuralConnection, self).render_attributes() 72 | attrs.update({ 73 | 'src': self.conn.src, 74 | 'dst': self.conn.dst, 75 | 'weight': nf(self.conn.weight) 76 | }) 77 | 78 | return attrs 79 | -------------------------------------------------------------------------------- /pyrevolve/build/util.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utility functions 3 | """ 4 | from __future__ import absolute_import 5 | 6 | size_scale_factor = 1 7 | weight_scale_factor = 1 8 | 9 | 10 | def in_mm(x): 11 | return size_scale_factor * x / 1000.0 12 | 13 | 14 | def in_cm(x): 15 | return size_scale_factor * x / 100.0 16 | 17 | 18 | def in_grams(x): 19 | return weight_scale_factor * x / 1000.0 20 | -------------------------------------------------------------------------------- /pyrevolve/convert/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .yaml import yaml_to_proto 4 | from .yaml import proto_to_yaml 5 | -------------------------------------------------------------------------------- /pyrevolve/custom_logging/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | -------------------------------------------------------------------------------- /pyrevolve/custom_logging/logger.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | import logging 4 | import sys 5 | 6 | 7 | def create_logger(name='revolve', level=logging.DEBUG, handlers=None): 8 | _logger = logging.getLogger(name) 9 | _logger.setLevel(level) 10 | handlers = logging.StreamHandler(sys.stdout) if handlers is None else handlers 11 | handlers = [handlers] if type(handlers) is not list else handlers 12 | for handler in handlers: 13 | _console_handler = handler 14 | _console_handler.setLevel(level) 15 | _revolve_formatter = logging.Formatter('[%(asctime)s %(name)10s] %(levelname)-8s %(message)s') 16 | _console_handler.setFormatter(_revolve_formatter) 17 | _logger.addHandler(_console_handler) 18 | return _logger 19 | 20 | 21 | # General logger to standard output 22 | logger = create_logger( 23 | name='revolve', 24 | level=logging.DEBUG, 25 | handlers=[logging.StreamHandler(sys.stdout), logging.FileHandler('./revolve.log', mode='a')] 26 | ) 27 | 28 | # Genotype logger for logging mutation and crossover details to a file 29 | genotype_logger = create_logger( 30 | name='genotype', 31 | level=logging.INFO, 32 | handlers=logging.FileHandler('./genotype.log', mode='a') 33 | ) 34 | -------------------------------------------------------------------------------- /pyrevolve/data_analisys/visualize_robot.py: -------------------------------------------------------------------------------- 1 | import asyncio 2 | import logging 3 | import sys 4 | import os 5 | 6 | from pyrevolve import parser 7 | from pyrevolve.custom_logging import logger 8 | from pyrevolve.revolve_bot import RevolveBot 9 | from pyrevolve.SDF.math import Vector3 10 | from pyrevolve.tol.manage import World 11 | from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor 12 | from pyrevolve.evolution import fitness 13 | 14 | 15 | async def test_robot_run(robot_file_path: str): 16 | log = logger.create_logger('experiment', handlers=[ 17 | logging.StreamHandler(sys.stdout), 18 | ]) 19 | 20 | # Set debug level to DEBUG 21 | log.setLevel(logging.DEBUG) 22 | 23 | # Parse command line / file input arguments 24 | settings = parser.parse_args() 25 | 26 | # Start Simulator 27 | if settings.simulator_cmd != 'debug': 28 | simulator_supervisor = DynamicSimSupervisor( 29 | world_file=settings.world, 30 | simulator_cmd=settings.simulator_cmd, 31 | simulator_args=["--verbose"], 32 | plugins_dir_path=os.path.join('.', 'build', 'lib'), 33 | models_dir_path=os.path.join('.', 'models'), 34 | simulator_name='gazebo' 35 | ) 36 | await simulator_supervisor.launch_simulator(port=settings.port_start) 37 | await asyncio.sleep(0.1) 38 | 39 | # Connect to the simulator and pause 40 | connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) 41 | await asyncio.sleep(1) 42 | 43 | # init finished 44 | 45 | robot = RevolveBot(_id=settings.test_robot) 46 | robot.load_file(robot_file_path, conf_type='yaml') 47 | robot.save_file(f'{robot_file_path}.sdf', conf_type='sdf') 48 | 49 | await connection.pause(True) 50 | robot_manager = await connection.insert_robot(robot, Vector3(0, 0, 0.25), life_timeout=None) 51 | await asyncio.sleep(1.0) 52 | 53 | # Start the main life loop 54 | while True: 55 | # Print robot fitness every second 56 | status = 'dead' if robot_manager.dead else 'alive' 57 | print(f"Robot fitness ({status}) is \n" 58 | f" OLD: {fitness.online_old_revolve(robot_manager)}\n" 59 | f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n" 60 | f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}") 61 | await asyncio.sleep(1.0) 62 | -------------------------------------------------------------------------------- /pyrevolve/evolution/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/evolution/__init__.py -------------------------------------------------------------------------------- /pyrevolve/evolution/individual.py: -------------------------------------------------------------------------------- 1 | # (G,P) 2 | 3 | 4 | class Individual: 5 | def __init__(self, genotype, phenotype=None): 6 | """ 7 | Creates an Individual object with the given genotype and optionally the phenotype. 8 | 9 | :param genotype: genotype of the individual 10 | :param phenotype (optional): phenotype of the individual 11 | """ 12 | self.genotype = genotype 13 | self.phenotype = phenotype 14 | self.fitness = None 15 | self.parents = None 16 | self.failed_eval_attempt_count = 0 17 | 18 | def develop(self): 19 | """ 20 | Develops genotype into a intermediate phenotype 21 | 22 | """ 23 | if self.phenotype is None: 24 | self.phenotype = self.genotype.develop() 25 | 26 | @property 27 | def id(self): 28 | _id = None 29 | if self.phenotype is not None: 30 | _id = self.phenotype.id 31 | elif self.genotype.id is not None: 32 | _id = self.genotype.id 33 | return _id 34 | 35 | def export_genotype(self, folder): 36 | self.genotype.export_genotype(f'{folder}/genotypes/genotype_{self.phenotype.id}.txt') 37 | 38 | def export_phenotype(self, folder): 39 | if self.phenotype is not None: 40 | self.phenotype.save_file(f'{folder}/phenotypes/{self.phenotype.id}.yaml', conf_type='yaml') 41 | 42 | def export_fitness(self, folder): 43 | """ 44 | It's saving the fitness into a file. The fitness can be a floating point number or None 45 | :param folder: folder where to save the fitness 46 | """ 47 | with open(f'{folder}/fitness_{self.id}.txt', 'w') as f: 48 | f.write(str(self.fitness)) 49 | 50 | def export(self, folder): 51 | self.export_genotype(folder) 52 | self.export_phenotype(folder) 53 | self.export_fitness(folder) 54 | 55 | def __repr__(self): 56 | return f'Individual_{self.id}({self.fitness})' 57 | -------------------------------------------------------------------------------- /pyrevolve/evolution/pop_management/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/evolution/pop_management/__init__.py -------------------------------------------------------------------------------- /pyrevolve/evolution/pop_management/generational.py: -------------------------------------------------------------------------------- 1 | def generational_population_management(old_individuals, new_individuals): 2 | assert (len(old_individuals) == len(new_individuals)) 3 | return new_individuals 4 | -------------------------------------------------------------------------------- /pyrevolve/evolution/pop_management/steady_state.py: -------------------------------------------------------------------------------- 1 | from pyrevolve.evolution.selection import multiple_selection 2 | 3 | 4 | def steady_state_population_management(old_individuals, new_individuals, selector): 5 | pop_size = len(old_individuals) 6 | selection_pool = old_individuals + new_individuals 7 | 8 | return multiple_selection(selection_pool, pop_size, selector) 9 | -------------------------------------------------------------------------------- /pyrevolve/evolution/selection.py: -------------------------------------------------------------------------------- 1 | from random import randint 2 | 3 | _neg_inf = -float('Inf') 4 | 5 | 6 | def _compare_maj_fitness(indiv_1, indiv_2): 7 | fit_1 = _neg_inf if indiv_1.fitness is None else indiv_1.fitness 8 | fit_2 = _neg_inf if indiv_2.fitness is None else indiv_2.fitness 9 | return fit_1 > fit_2 10 | 11 | 12 | def tournament_selection(population, k=2): 13 | """ 14 | Perform tournament selection and return best individual 15 | :param k: amount of individuals to participate in tournament 16 | """ 17 | best_individual = None 18 | for _ in range(k): 19 | individual = population[randint(0, len(population) - 1)] 20 | if (best_individual is None) or (_compare_maj_fitness(individual, best_individual)): 21 | best_individual = individual 22 | return best_individual 23 | 24 | 25 | def multiple_selection(population, selection_size, selection_function): 26 | """ 27 | Perform selection on population of distinct group, can be used in the form parent selection or survival selection 28 | :param population: parent selection in population 29 | :param selection_size: amount of indivuals to select 30 | :param selection_function: 31 | """ 32 | assert (len(population) >= selection_size) 33 | selected_individuals = [] 34 | for _ in range(selection_size): 35 | new_individual = False 36 | while new_individual is False: 37 | selected_individual = selection_function(population) 38 | if selected_individual not in selected_individuals: 39 | selected_individuals.append(selected_individual) 40 | new_individual = True 41 | return selected_individuals 42 | -------------------------------------------------------------------------------- /pyrevolve/examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/examples/__init__.py -------------------------------------------------------------------------------- /pyrevolve/examples/analyze_body.py: -------------------------------------------------------------------------------- 1 | """ 2 | Generates a bot using the code in `generated_sdf`, 3 | and sends it to the body analyzer to have it analyzed. 4 | 5 | If the analysis is accepted, it outputs the bot, otherwise 6 | it generates a new one. Writes the final bot's contents to 7 | stdout, statistics are written to stderr. 8 | """ 9 | from __future__ import absolute_import 10 | from __future__ import print_function 11 | 12 | import sys 13 | import random 14 | 15 | from pyrevolve.sdfbuilder.math import Vector3 16 | from .generated_sdf import generate_robot, builder, robot_to_sdf 17 | from ..gazebo import get_analysis_robot, BodyAnalyzer 18 | from ..custom_logging.logger import logger 19 | 20 | import asyncio 21 | 22 | 23 | if len(sys.argv) > 1: 24 | seed = int(sys.argv[1]) 25 | else: 26 | seed = random.randint(0, 10000) 27 | 28 | random.seed(seed) 29 | logger.info("Seed: {}".format(seed)) 30 | 31 | 32 | async def analysis_func(): 33 | analyzer = await (BodyAnalyzer.create(address=("127.0.0.1", 11346))) 34 | 35 | # Try a maximum of 100 times 36 | for _ in range(100): 37 | # Generate a new robot 38 | robot = generate_robot() 39 | 40 | sdf = get_analysis_robot(robot, builder) 41 | 42 | # Find out its intersections and bounding box 43 | intersections, bbox = await ( 44 | analyzer.analyze_robot(robot, builder=builder)) 45 | 46 | if intersections: 47 | logger.info("Invalid model - intersections detected.", file=sys.stderr) 48 | else: 49 | logger.info("No model intersections detected!", file=sys.stderr) 50 | if bbox: 51 | # Translate the model in z direction so it stands directly on 52 | # the ground 53 | logger.info("Model bounding box: ({}, {}, {}), ({}, {}, {})".format( 54 | bbox.min.x, 55 | bbox.min.y, 56 | bbox.min.z, 57 | bbox.max.x, 58 | bbox.max.y, 59 | bbox.max.z 60 | ), file=sys.stderr) 61 | model = sdf.elements[0] 62 | model.translate(Vector3(0, 0, -bbox.min.z)) 63 | 64 | logger.info(str(robot_to_sdf(robot, "test_bot", "controllerplugin.so"))) 65 | break 66 | 67 | loop = asyncio.get_event_loop() 68 | loop.run_until_complete(analysis_func()) 69 | -------------------------------------------------------------------------------- /pyrevolve/examples/from_yaml.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import print_function 3 | 4 | from pyrevolve.build.sdf import RobotBuilder 5 | from pyrevolve.build.sdf import BodyBuilder 6 | from pyrevolve.build.sdf import NeuralNetBuilder 7 | from pyrevolve.convert import yaml_to_proto 8 | from pyrevolve.sdfbuilder import SDF 9 | from pyrevolve.sdfbuilder.math import Vector3 10 | 11 | from .generated_sdf import body_spec, brain_spec 12 | 13 | from ..custom_logging.logger import logger 14 | 15 | bot_yaml = ''' 16 | --- 17 | body: 18 | id : Core 19 | type : Core 20 | children: 21 | 1: 22 | id: Hinge 23 | type: Hinge 24 | params: 25 | length: 0.5 26 | red: 1.0 27 | green: 0.0 28 | blue: 0.0 29 | 4: 30 | id: Wheel 31 | type: Wheel 32 | params: 33 | red: 0.0 34 | green: 1.0 35 | blue: 0.0 36 | 5: 37 | id: Wheel2 38 | type: Wheel 39 | params: 40 | red: 0.0 41 | green: 1.0 42 | blue: 0.0 43 | brain: 44 | params: 45 | Wheel-out-0: 46 | type: Oscillator 47 | period: 3 48 | Wheel2-out-0: 49 | type: Oscillator 50 | period: 3 51 | ''' 52 | 53 | bot = yaml_to_proto(body_spec, brain_spec, bot_yaml) 54 | builder = RobotBuilder(BodyBuilder(body_spec), NeuralNetBuilder(brain_spec)) 55 | model = builder.sdf_robot(bot, "libRobotControlPlugin.so") 56 | model.translate(Vector3(0, 0, 0.5)) 57 | sdf = SDF() 58 | sdf.add_element(model) 59 | logger.info(str(sdf)) 60 | -------------------------------------------------------------------------------- /pyrevolve/gazebo/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | # from .analyze import BodyAnalyzer 4 | # from .analyze import analyze_body 5 | # from .analyze import get_analysis_robot 6 | 7 | from .connect import connect, RequestHandler 8 | -------------------------------------------------------------------------------- /pyrevolve/gazebo/manage/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .world import WorldManager 4 | -------------------------------------------------------------------------------- /pyrevolve/generate/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/generate/__init__.py -------------------------------------------------------------------------------- /pyrevolve/genotype/__init__.py: -------------------------------------------------------------------------------- 1 | from .genotype import Genotype -------------------------------------------------------------------------------- /pyrevolve/genotype/genotype.py: -------------------------------------------------------------------------------- 1 | class Genotype: 2 | def clone(self): 3 | """ 4 | Create an returns deep copy of the genotype 5 | """ 6 | raise NotImplementedError("Method must be implemented by genome") 7 | 8 | def develop(self): 9 | """ 10 | Develops the genome into a revolve_bot (proto-phenotype) 11 | 12 | :return: a RevolveBot instance 13 | :rtype: RevolveBot 14 | """ 15 | raise NotImplementedError("Method must be implemented by genome") 16 | 17 | 18 | class GenotypeConfig: 19 | def __init__(self, 20 | e_max_groups, 21 | axiom_w, 22 | i_iterations, 23 | weight_min, 24 | weight_max, 25 | oscillator_param_min, 26 | oscillator_param_max): 27 | self.e_max_groups = e_max_groups 28 | self.axiom_w = axiom_w 29 | self.i_iterations = i_iterations 30 | self.weight_min = weight_min 31 | self.weight_max = weight_max 32 | self.oscillator_param_min = oscillator_param_min 33 | self.oscillator_param_max = oscillator_param_max 34 | -------------------------------------------------------------------------------- /pyrevolve/genotype/plasticoding/__init__.py: -------------------------------------------------------------------------------- 1 | from .plasticoding import Plasticoding -------------------------------------------------------------------------------- /pyrevolve/genotype/plasticoding/crossover/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/genotype/plasticoding/crossover/__init__.py -------------------------------------------------------------------------------- /pyrevolve/genotype/plasticoding/crossover/crossover.py: -------------------------------------------------------------------------------- 1 | class CrossoverConfig: 2 | def __init__(self, 3 | crossover_prob): 4 | """ 5 | Creates a Crossover object that sets the configuration for the crossover operator 6 | 7 | :param crossover_prob: crossover probability 8 | """ 9 | self.crossover_prob = crossover_prob 10 | -------------------------------------------------------------------------------- /pyrevolve/genotype/plasticoding/crossover/standard_crossover.py: -------------------------------------------------------------------------------- 1 | from pyrevolve.genotype.plasticoding.plasticoding import Plasticoding, Alphabet, PlasticodingConfig 2 | from pyrevolve.evolution.individual import Individual 3 | import random 4 | from ....custom_logging.logger import genotype_logger 5 | 6 | 7 | def generate_child_genotype(parent_genotypes, genotype_conf, crossover_conf): 8 | """ 9 | Generates a child (individual) by randomly mixing production rules from two parents 10 | 11 | :param parents: parents to be used for crossover 12 | 13 | :return: child genotype 14 | """ 15 | grammar = {} 16 | crossover_attempt = random.uniform(0.0, 1.0) 17 | if crossover_attempt > crossover_conf.crossover_prob: 18 | grammar = parent_genotypes[0].grammar 19 | else: 20 | for letter in Alphabet.modules(): 21 | parent = random.randint(0, 1) 22 | # gets the production rule for the respective letter 23 | grammar[letter[0]] = parent_genotypes[parent].grammar[letter[0]] 24 | 25 | genotype = Plasticoding(genotype_conf, 'tmp') 26 | genotype.grammar = grammar 27 | return genotype.clone() 28 | 29 | 30 | def standard_crossover(parent_individuals, genotype_conf, crossover_conf): 31 | """ 32 | Creates an child (individual) through crossover with two parents 33 | 34 | :param parent_genotypes: genotypes of the parents to be used for crossover 35 | :return: genotype result of the crossover 36 | """ 37 | parent_genotypes = [p.genotype for p in parent_individuals] 38 | new_genotype = generate_child_genotype(parent_genotypes, genotype_conf, crossover_conf) 39 | #TODO what if you have more than 2 parents? fix log 40 | genotype_logger.info( 41 | f'crossover: for genome {new_genotype.id} - p1: {parent_genotypes[0].id} p2: {parent_genotypes[1].id}.') 42 | return new_genotype 43 | -------------------------------------------------------------------------------- /pyrevolve/genotype/plasticoding/mutation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/genotype/plasticoding/mutation/__init__.py -------------------------------------------------------------------------------- /pyrevolve/genotype/plasticoding/mutation/mutation.py: -------------------------------------------------------------------------------- 1 | class MutationConfig: 2 | def __init__(self, 3 | mutation_prob, 4 | genotype_conf): 5 | """ 6 | Creates a MutationConfig object that sets the configuration for the mutation operator 7 | 8 | :param mutation_prob: mutation probability 9 | :param genotype_conf: configuration for the genotype to be mutated 10 | """ 11 | self.mutation_prob = mutation_prob 12 | self.genotype_conf = genotype_conf 13 | -------------------------------------------------------------------------------- /pyrevolve/revolve_bot/__init__.py: -------------------------------------------------------------------------------- 1 | # Adds OrderedDict as possible yaml input 2 | import yaml 3 | from collections import OrderedDict 4 | 5 | from .revolve_bot import RevolveBot 6 | from .revolve_module import RevolveModule, BoxSlot 7 | 8 | 9 | def represent_ordereddict(dumper, data): 10 | value = [] 11 | 12 | for item_key, item_value in data.items(): 13 | node_key = dumper.represent_data(item_key) 14 | node_value = dumper.represent_data(item_value) 15 | 16 | value.append((node_key, node_value)) 17 | 18 | return yaml.nodes.MappingNode(u'tag:yaml.org,2002:map', value) 19 | 20 | yaml.add_representer(OrderedDict, represent_ordereddict) 21 | -------------------------------------------------------------------------------- /pyrevolve/revolve_bot/brain/__init__.py: -------------------------------------------------------------------------------- 1 | from .base import Brain 2 | from .brain_nn import BrainNN 3 | from .rlpower_splines import BrainRLPowerSplines 4 | from .bo_cpg import BrainCPGBO 5 | -------------------------------------------------------------------------------- /pyrevolve/revolve_bot/brain/base.py: -------------------------------------------------------------------------------- 1 | import pyrevolve.revolve_bot.brain 2 | 3 | 4 | class Brain(object): 5 | 6 | @staticmethod 7 | def from_yaml(yaml_brain): 8 | brain_type = yaml_brain['type'] 9 | 10 | if brain_type == pyrevolve.revolve_bot.brain.BrainNN.TYPE: 11 | return pyrevolve.revolve_bot.brain.BrainNN.from_yaml(yaml_brain) 12 | elif brain_type == pyrevolve.revolve_bot.brain.BrainRLPowerSplines.TYPE: 13 | return pyrevolve.revolve_bot.brain.BrainRLPowerSplines.from_yaml(yaml_brain) 14 | elif brain_type == pyrevolve.revolve_bot.brain.BrainCPGBO.TYPE: 15 | return pyrevolve.revolve_bot.brain.BrainCPGBO.from_yaml(yaml_brain) 16 | else: 17 | return Brain() 18 | 19 | def to_yaml(self): 20 | return {} 21 | 22 | def learner_sdf(self): 23 | return None 24 | 25 | def controller_sdf(self): 26 | return None 27 | -------------------------------------------------------------------------------- /pyrevolve/revolve_bot/brain/rlpower_splines.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree 2 | from .base import Brain 3 | 4 | 5 | class BrainRLPowerSplines(Brain): 6 | TYPE = 'rlpower-splines' 7 | 8 | def __init__(self, evaluation_rate=30.0): 9 | self._evaluation_rate = evaluation_rate 10 | 11 | @staticmethod 12 | def from_yaml(yaml_object): 13 | return BrainRLPowerSplines() 14 | 15 | def to_yaml(self): 16 | return { 17 | 'type': self.TYPE 18 | } 19 | 20 | def learner_sdf(self): 21 | return xml.etree.ElementTree.Element('rv:learner', { 22 | 'type': 'rlpower', 23 | 'evaluation_rate': str(self._evaluation_rate), 24 | }) 25 | 26 | def controller_sdf(self): 27 | return xml.etree.ElementTree.Element('rv:controller', {'type': 'spline'}) 28 | -------------------------------------------------------------------------------- /pyrevolve/revolve_bot/render/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/revolve_bot/render/__init__.py -------------------------------------------------------------------------------- /pyrevolve/spec/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .exception import RobotSpecificationException 4 | 5 | from .implementation import BodyImplementation 6 | from .implementation import NeuralNetImplementation 7 | from .implementation import PartSpec 8 | from .implementation import NeuronSpec 9 | from .implementation import ParamSpec 10 | from .implementation import NormalDistParamSpec 11 | from .implementation import default_neural_net 12 | 13 | from .msgs import * 14 | 15 | from .validate import BodyValidator 16 | from .validate import NeuralNetValidator 17 | from .validate import RobotValidator 18 | from .validate import Validator 19 | -------------------------------------------------------------------------------- /pyrevolve/spec/exception.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | 4 | class RobotSpecificationException(BaseException): 5 | def __init__(self, msg): 6 | super(RobotSpecificationException, self).__init__(msg) 7 | 8 | 9 | def err(msg): 10 | """ 11 | Simple internal error function 12 | :param msg: 13 | :return: 14 | """ 15 | raise RobotSpecificationException("Error: {}".format(msg)) 16 | -------------------------------------------------------------------------------- /pyrevolve/spec/msgs/__init__.py: -------------------------------------------------------------------------------- 1 | from .robot_pb2 import * 2 | from .body_pb2 import * 3 | from .parameter_pb2 import * 4 | from .neural_net_pb2 import * 5 | from .sdf_body_analyze_pb2 import * 6 | from .model_inserted_pb2 import * 7 | from .robot_states_pb2 import * 8 | -------------------------------------------------------------------------------- /pyrevolve/spec/msgs/parameter_pb2.py: -------------------------------------------------------------------------------- 1 | # Generated by the protocol buffer compiler. DO NOT EDIT! 2 | # source: parameter.proto 3 | 4 | import sys 5 | _b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import message as _message 8 | from google.protobuf import reflection as _reflection 9 | from google.protobuf import symbol_database as _symbol_database 10 | # @@protoc_insertion_point(imports) 11 | 12 | _sym_db = _symbol_database.Default() 13 | 14 | 15 | 16 | 17 | DESCRIPTOR = _descriptor.FileDescriptor( 18 | name='parameter.proto', 19 | package='revolve.msgs', 20 | syntax='proto2', 21 | serialized_options=None, 22 | serialized_pb=_b('\n\x0fparameter.proto\x12\x0crevolve.msgs\"\x1a\n\tParameter\x12\r\n\x05value\x18\x01 \x02(\x01') 23 | ) 24 | 25 | 26 | 27 | 28 | _PARAMETER = _descriptor.Descriptor( 29 | name='Parameter', 30 | full_name='revolve.msgs.Parameter', 31 | filename=None, 32 | file=DESCRIPTOR, 33 | containing_type=None, 34 | fields=[ 35 | _descriptor.FieldDescriptor( 36 | name='value', full_name='revolve.msgs.Parameter.value', index=0, 37 | number=1, type=1, cpp_type=5, label=2, 38 | has_default_value=False, default_value=float(0), 39 | message_type=None, enum_type=None, containing_type=None, 40 | is_extension=False, extension_scope=None, 41 | serialized_options=None, file=DESCRIPTOR), 42 | ], 43 | extensions=[ 44 | ], 45 | nested_types=[], 46 | enum_types=[ 47 | ], 48 | serialized_options=None, 49 | is_extendable=False, 50 | syntax='proto2', 51 | extension_ranges=[], 52 | oneofs=[ 53 | ], 54 | serialized_start=33, 55 | serialized_end=59, 56 | ) 57 | 58 | DESCRIPTOR.message_types_by_name['Parameter'] = _PARAMETER 59 | _sym_db.RegisterFileDescriptor(DESCRIPTOR) 60 | 61 | Parameter = _reflection.GeneratedProtocolMessageType('Parameter', (_message.Message,), dict( 62 | DESCRIPTOR = _PARAMETER, 63 | __module__ = 'parameter_pb2' 64 | # @@protoc_insertion_point(class_scope:revolve.msgs.Parameter) 65 | )) 66 | _sym_db.RegisterMessage(Parameter) 67 | 68 | 69 | # @@protoc_insertion_point(module_scope) 70 | -------------------------------------------------------------------------------- /pyrevolve/tol/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .logger import logger 4 | from .logger import output_console 5 | from .logger import log_debug 6 | -------------------------------------------------------------------------------- /pyrevolve/tol/build/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .builder import get_builder 4 | from .builder import to_sdfbot 5 | -------------------------------------------------------------------------------- /pyrevolve/tol/build/builder.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from pyrevolve.build.sdf import RobotBuilder, BodyBuilder, NeuralNetBuilder, BasicBattery 4 | from pyrevolve.angle.robogen.util import apply_surface_parameters 5 | from pyrevolve.sdfbuilder import SDF 6 | from ..spec import get_body_spec, get_brain_spec 7 | 8 | 9 | def get_builder(conf): 10 | """ 11 | :param conf: 12 | :return: 13 | """ 14 | body_spec = get_body_spec(conf) 15 | brain_spec = get_brain_spec(conf) 16 | return RobotBuilder( 17 | body_builder=BodyBuilder(body_spec, conf), 18 | brain_builder=NeuralNetBuilder(brain_spec) 19 | ) 20 | 21 | 22 | def to_sdfbot( 23 | robot, 24 | name, 25 | builder, 26 | conf, 27 | battery_charge=None 28 | ): 29 | """ 30 | :param robot: 31 | :param name: 32 | :param builder: 33 | :param conf: Config 34 | :param battery_charge: 35 | :return: 36 | """ 37 | battery = None if battery_charge is None else BasicBattery(battery_charge) 38 | brain_conf = None if not hasattr(conf, 'brain_conf') else conf.brain_conf 39 | 40 | model = builder.sdf_robot( 41 | robot=robot, 42 | controller_plugin="libRobotControlPlugin.so", 43 | update_rate=conf.controller_update_rate, 44 | name=name, 45 | battery=battery, 46 | brain_conf=brain_conf) 47 | 48 | apply_surface_parameters(model, conf.world_step_size) 49 | 50 | sdf = SDF() 51 | sdf.add_element(model) 52 | return sdf 53 | -------------------------------------------------------------------------------- /pyrevolve/tol/constants.py: -------------------------------------------------------------------------------- 1 | """ 2 | ToL constants 3 | """ 4 | from __future__ import absolute_import 5 | 6 | MAX_HIDDEN_NEURONS = 10 7 | """ Maximum number of hidden neurons """ 8 | 9 | # Thickness and height of the arena walls in meters 10 | WALL_THICKNESS = 0.05 11 | WALL_HEIGHT = 1.0 12 | -------------------------------------------------------------------------------- /pyrevolve/tol/logger.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | import logging 4 | 5 | logger = logging.getLogger("tol") 6 | 7 | 8 | def output_console(): 9 | logger.addHandler(logging.StreamHandler()) 10 | 11 | 12 | def log_debug(): 13 | logger.setLevel(logging.DEBUG) 14 | -------------------------------------------------------------------------------- /pyrevolve/tol/manage/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .world import World 4 | -------------------------------------------------------------------------------- /pyrevolve/tol/scenery/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | # from .birth_clinic import BirthClinic 4 | 5 | from .wall import Wall 6 | -------------------------------------------------------------------------------- /pyrevolve/tol/scenery/wall.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from pyrevolve.sdfbuilder import Model, Posable, Link 4 | from pyrevolve.SDF.math import Vector3 5 | 6 | 7 | class Wall(Model): 8 | """ 9 | Simple wall model to wall off the 10 | """ 11 | 12 | def __init__(self, name, start, end, thickness, height, **kwargs): 13 | """ 14 | Construct a wall of the given thickness and height from 15 | `start` to `end`. This simply results in a box. 16 | 17 | :param start: Starting point of the wall. 18 | :type start: Vector3 19 | :param end: Ending point of the wall. 20 | :type end: Vector3 21 | :return: 22 | """ 23 | super(Wall, self).__init__(name, static=True, **kwargs) 24 | if start.z != end.z: 25 | raise AssertionError( 26 | "Walls with different start / end z-axis are undefined.") 27 | 28 | center = 0.5 * (end + start) 29 | diff = end - start 30 | size = abs(diff) 31 | self.wall = Link("wall_link") 32 | self.wall.make_box(10e10, size, thickness, height) 33 | 34 | # Rotate the wall so it aligns with the vector from 35 | # x to y 36 | self.align( 37 | Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 0, 1), 38 | center, diff, Vector3(0, 0, 1), Posable("mock") 39 | ) 40 | 41 | self.add_element(self.wall) 42 | -------------------------------------------------------------------------------- /pyrevolve/tol/spec/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .body import get_body_spec 4 | from .body import get_body_generator 5 | 6 | from .brain import get_brain_spec 7 | from .brain import get_brain_generator 8 | 9 | from .robot import get_tree_generator 10 | -------------------------------------------------------------------------------- /pyrevolve/tol/spec/body.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from random import gauss 4 | 5 | from pyrevolve.angle.robogen.spec import BodyGenerator 6 | from pyrevolve.angle.robogen.spec import get_body_spec as rv_body_spec 7 | 8 | 9 | class BodyGen(BodyGenerator): 10 | def choose_max_parts(self): 11 | return min(self.conf.max_parts, 12 | max(self.conf.min_parts, 13 | int(gauss(self.conf.initial_parts_mu, 14 | self.conf.initial_parts_sigma)))) 15 | 16 | 17 | def get_body_spec(conf): 18 | return rv_body_spec(conf) 19 | 20 | 21 | def get_body_generator(conf): 22 | """ 23 | Returns a body generator. 24 | 25 | :param conf: 26 | :return: 27 | """ 28 | return BodyGen(conf) 29 | -------------------------------------------------------------------------------- /pyrevolve/tol/spec/brain.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from pyrevolve.generate import NeuralNetworkGenerator 4 | from pyrevolve.spec import default_neural_net 5 | 6 | from .. import constants 7 | 8 | 9 | def get_brain_spec(conf): 10 | """ 11 | Returns the brain specification corresponding to the 12 | given config. 13 | :param conf: 14 | :return: 15 | """ 16 | return default_neural_net(conf.brain_mutation_epsilon) 17 | 18 | 19 | def get_brain_generator(conf): 20 | """ 21 | Returns a brain generator. 22 | 23 | :param conf: 24 | :return: 25 | """ 26 | return NeuralNetworkGenerator( 27 | get_brain_spec(conf), 28 | max_hidden=constants.MAX_HIDDEN_NEURONS, 29 | conn_prob=conf.p_connect_neurons 30 | ) 31 | -------------------------------------------------------------------------------- /pyrevolve/tol/spec/robot.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from pyrevolve.angle.robogen.spec import RobogenTreeGenerator 4 | from ..spec.body import get_body_generator 5 | from ..spec.brain import get_brain_generator 6 | 7 | 8 | def get_tree_generator(conf): 9 | """ 10 | :param conf: 11 | :return: 12 | :rtype: TreeGenerator 13 | """ 14 | body_gen = get_body_generator(conf) 15 | brain_gen = get_brain_generator(conf) 16 | return RobogenTreeGenerator(body_gen, brain_gen, conf) 17 | -------------------------------------------------------------------------------- /pyrevolve/tol/util/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/pyrevolve/tol/util/__init__.py -------------------------------------------------------------------------------- /pyrevolve/tol/util/analyze.py: -------------------------------------------------------------------------------- 1 | """ 2 | Includes functions for robot tree analysis, to: 3 | 4 | - Determine the number of extremities per robot 5 | - Determine the number of joints per robot 6 | - 7 | """ 8 | from __future__ import absolute_import 9 | 10 | import itertools 11 | 12 | 13 | def count_extremities(node): 14 | """ 15 | Counts the extremities in the subtree represented by the given node. 16 | :type node: Node 17 | """ 18 | # Count all leaf nodes to get the extremities 19 | if node.is_leaf(): 20 | return 1 21 | else: 22 | return len([c for c in node.get_children() 23 | if not len(list(c.child_connections()))]) 24 | 25 | 26 | def count_types(node, types): 27 | """ 28 | :type node: Node 29 | :param node: 30 | :type types: tuple 31 | """ 32 | base = 1 if node.part.type in types else 0 33 | return base + len([c for c in node.get_children() if c.part.type in types]) 34 | 35 | 36 | def count_joints(node): 37 | return count_types(node, ("ActiveHinge", "Hinge")) 38 | 39 | 40 | def count_motors(node): 41 | return count_types(node, ("ActiveHinge",)) 42 | 43 | 44 | def list_extremities(node): 45 | """ 46 | Returns an iterator with the first node of each 47 | extremity in the subtree represented by the given node. 48 | """ 49 | # If the list of extremities on this node is 1, return this node 50 | if count_extremities(node) == 1: 51 | return [node] 52 | 53 | # Otherwise return the starting point for all extremities on child nodes 54 | return list(itertools.chain(*(list_extremities(c.node) 55 | for c in node.child_connections()))) 56 | 57 | 58 | def joints_per_extremity(node): 59 | """ 60 | Returns a list with the number of joints encountered in each extremity 61 | in the subtree represented by the given node. 62 | :type node: Node 63 | """ 64 | return [count_extremities(c) for c in list_extremities(node)] 65 | 66 | 67 | def count_connections(node): 68 | """ 69 | :type node: Node 70 | """ 71 | return len(node.get_neural_connections()) + \ 72 | sum(count_connections(c.node) for c in node.child_connections()) 73 | -------------------------------------------------------------------------------- /pyrevolve/util/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .time import Time 4 | 5 | from .functions import * 6 | from .futures import * 7 | from .supervisor import Supervisor 8 | -------------------------------------------------------------------------------- /pyrevolve/util/asyncio_timer.py: -------------------------------------------------------------------------------- 1 | import asyncio 2 | 3 | 4 | class AsyncTimer: 5 | def __init__(self, timeout, callback): 6 | self._timeout = timeout 7 | self._callback = callback 8 | self._task = asyncio.ensure_future(self._job()) 9 | self._stop = False 10 | 11 | async def _job(self): 12 | await asyncio.sleep(self._timeout) 13 | await self._callback() 14 | if not self._stop: 15 | self._task = asyncio.ensure_future(self._job()) 16 | 17 | def cancel(self): 18 | self._task.cancel() 19 | self._stop = True 20 | -------------------------------------------------------------------------------- /pyrevolve/util/functions.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | import random 4 | 5 | 6 | def decide(probability): 7 | """ 8 | Returns `True` with the given probability 9 | :param probability: Probability between 0 and 1 10 | :type probability: float 11 | :return: 12 | """ 13 | return random.random() < probability 14 | -------------------------------------------------------------------------------- /pyrevolve/util/futures.py: -------------------------------------------------------------------------------- 1 | """ 2 | Multi-future function to amend that missing functionality 3 | in Trollius. Code taken mostly from Tornado. 4 | """ 5 | from __future__ import absolute_import 6 | from __future__ import print_function 7 | 8 | import sys 9 | 10 | from asyncio import Future 11 | from ..custom_logging.logger import logger 12 | 13 | 14 | def multi_future(children, quiet_exceptions=()): 15 | """ 16 | Wraps multiple futures in a single future. 17 | :param quiet_exceptions: 18 | :param children: 19 | """ 20 | if isinstance(children, dict): 21 | keys = list(children.keys()) 22 | children = list(children.values()) 23 | else: 24 | keys = None 25 | unfinished_children = set(children) 26 | 27 | future = Future() 28 | if not children: 29 | future.set_result({} if keys is not None else []) 30 | 31 | def callback(ft): 32 | unfinished_children.remove(ft) 33 | if not unfinished_children: 34 | result_list = [] 35 | for ft in children: 36 | try: 37 | result_list.append(ft.result()) 38 | except Exception as e: 39 | if future.done(): 40 | if not isinstance(e, quiet_exceptions): 41 | logger.error("Multiple exceptions in yield list") 42 | else: 43 | future.set_exception(sys.exc_info()) 44 | if not future.done(): 45 | if keys is not None: 46 | future.set_result(dict(list(zip(keys, result_list)))) 47 | else: 48 | future.set_result(result_list) 49 | 50 | listening = set() 51 | for f in children: 52 | if f not in listening: 53 | listening.add(f) 54 | f.add_done_callback(callback) 55 | 56 | return future 57 | -------------------------------------------------------------------------------- /pyrevolve/util/supervisor/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .supervisor import Supervisor 4 | -------------------------------------------------------------------------------- /pyrevolve/util/supervisor/analyzer_queue.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from pyrevolve.custom_logging.logger import logger 4 | from pyrevolve.gazebo.analyze import BodyAnalyzer 5 | from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue 6 | from pyrevolve.util.supervisor.supervisor_collision import CollisionSimSupervisor 7 | 8 | 9 | class AnalyzerQueue(SimulatorQueue): 10 | EVALUATION_TIMEOUT = 30 # seconds 11 | 12 | def __init__(self, n_cores: int, settings, port_start=11345, simulator_cmd='gzserver'): 13 | super(AnalyzerQueue, self).__init__(n_cores, settings, port_start, simulator_cmd) 14 | 15 | def _simulator_supervisor(self, simulator_name_postfix): 16 | return CollisionSimSupervisor( 17 | world_file=os.path.join('tools', 'analyzer', 'analyzer-world.world'), 18 | simulator_cmd=self._simulator_cmd, 19 | simulator_args=["--verbose"], 20 | plugins_dir_path=os.path.join('.', 'build', 'lib'), 21 | models_dir_path=os.path.join('.', 'models'), 22 | simulator_name=f'analyzer_{simulator_name_postfix}' 23 | ) 24 | 25 | async def _connect_to_simulator(self, settings, address, port): 26 | return await BodyAnalyzer.create(address, port) 27 | 28 | async def _evaluate_robot(self, simulator_connection, robot, conf): 29 | if robot.failed_eval_attempt_count == 3: 30 | logger.info(f'Robot {robot.phenotype.id} analyze failed (reached max attempt of 3), fitness set to None.') 31 | analyze_result = None 32 | return analyze_result 33 | else: 34 | analyze_result = await simulator_connection.analyze_robot(robot.phenotype) 35 | return analyze_result 36 | -------------------------------------------------------------------------------- /pyrevolve/util/supervisor/nbsr.py: -------------------------------------------------------------------------------- 1 | """ 2 | Non-blocking stream reader to read data from a subprocess 3 | without the supervisor stalling. Code taken from: 4 | 5 | http://eyalarubas.com/python-subproc-nonblock.html 6 | """ 7 | from __future__ import absolute_import 8 | 9 | from threading import Thread 10 | from queue import Queue, Empty 11 | 12 | 13 | class NonBlockingStreamReader(object): 14 | def __init__(self, stream, prefix=None): 15 | """ 16 | stream: the stream to read from. 17 | Usually a process' stdout or stderr. 18 | """ 19 | 20 | self._s = stream 21 | self._q = Queue() 22 | self._prefix = prefix 23 | 24 | def _populate_queue(stream, queue): 25 | """ 26 | Collect lines from 'stream' and put them in 'queue'. 27 | """ 28 | 29 | while True: 30 | line = stream.readline() 31 | if line and line != '\n' and 'ODE Message 3' not in line.decode(): 32 | queue.put(line) 33 | else: 34 | # This used to throw an exception, but we cannot 35 | # catch it, and in any case don't need it right now. 36 | break 37 | 38 | self._t = Thread(target=_populate_queue, 39 | args=(self._s, self._q)) 40 | self._t.daemon = True 41 | 42 | # start collecting lines from the stream 43 | self._t.start() 44 | 45 | def readline(self, timeout=None): 46 | try: 47 | line = self._q.get(block=timeout is not None, timeout=timeout).decode("utf-8") 48 | 49 | if self._prefix: 50 | return "[{}] {}".format(self._prefix, line) 51 | else: 52 | return line 53 | 54 | except Empty: 55 | return None 56 | -------------------------------------------------------------------------------- /pyrevolve/util/supervisor/stream.py: -------------------------------------------------------------------------------- 1 | class StreamEnded(Exception): 2 | pass 3 | 4 | 5 | class PrettyStreamReader(object): 6 | def __init__(self, stream): 7 | self._stream = stream 8 | 9 | async def readline(self): 10 | if self._stream.at_eof(): 11 | raise StreamEnded() 12 | line = await self._stream.readline() 13 | return line.decode('utf-8').strip() 14 | 15 | def at_eof(self): 16 | return self._stream.at_eof() 17 | -------------------------------------------------------------------------------- /pyrevolve/util/supervisor/supervisor_collision.py: -------------------------------------------------------------------------------- 1 | from .supervisor_multi import DynamicSimSupervisor 2 | 3 | 4 | class CollisionSimSupervisor(DynamicSimSupervisor): 5 | def __init__(self, 6 | world_file, 7 | output_directory=None, 8 | simulator_cmd="gzserver", 9 | simulator_args=None, 10 | restore_arg="--restore-directory", 11 | snapshot_world_file="snapshot.world", 12 | restore_directory=None, 13 | plugins_dir_path=None, 14 | models_dir_path=None, 15 | simulator_name='collision', 16 | process_terminated_callback=None, 17 | ): 18 | super().__init__(world_file, 19 | output_directory, 20 | simulator_cmd, 21 | simulator_args, 22 | restore_arg, 23 | snapshot_world_file, 24 | restore_directory, 25 | plugins_dir_path, 26 | models_dir_path, 27 | simulator_name, 28 | process_terminated_callback) 29 | 30 | async def _launch_simulator(self, 31 | ready_str="Body analyzer ready", 32 | output_tag="simulator", 33 | address='localhost', 34 | port=11345): 35 | return await super()._launch_simulator(ready_str, output_tag, address, port) 36 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | argparse>=1.2.1 2 | asyncio==3.4.3 3 | futures==3.0.2 4 | numpy>=1.9.2 5 | PyYAML>=3.11 6 | protobuf>=3.0.0 7 | psutil==5.6.6 8 | matplotlib 9 | pycairo>=1.18.0 10 | graphviz>=0.10.1 11 | joblib 12 | pandas 13 | neat-python>=0.92 14 | python-dateutil>=2.8.0 15 | 16 | -e git+https://github.com/ci-group/pygazebo.git@master#egg=pygazebo 17 | -------------------------------------------------------------------------------- /revolve.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import sys 4 | import asyncio 5 | import importlib 6 | 7 | from pyrevolve.data_analisys.visualize_robot import test_robot_run 8 | from pyrevolve.data_analisys.check_robot_collision import test_collision_robot 9 | from pyrevolve import parser 10 | from experiments.examples import only_gazebo 11 | 12 | here = os.path.dirname(os.path.abspath(__file__)) 13 | rvpath = os.path.abspath(os.path.join(here, '..', 'revolve')) 14 | sys.path.append(os.path.dirname(os.path.abspath(__file__))) 15 | 16 | 17 | def run(loop, arguments): 18 | if arguments.test_robot is not None: 19 | return loop.run_until_complete(test_robot_run(arguments.test_robot)) 20 | 21 | if arguments.test_robot_collision is not None: 22 | return loop.run_until_complete(test_collision_robot(arguments.test_robot_collision)) 23 | 24 | if arguments.manager is not None: 25 | # this split will give errors on windows 26 | manager_lib = os.path.splitext(arguments.manager)[0] 27 | manager_lib = '.'.join(manager_lib.split('/')) 28 | manager = importlib.import_module(manager_lib).run 29 | return loop.run_until_complete(manager()) 30 | else: 31 | # no test robot, no manager -> just run gazebo 32 | loop.run_until_complete(only_gazebo.run()) 33 | loop.run_forever() 34 | 35 | 36 | def main(): 37 | import traceback 38 | 39 | def handler(_loop, context): 40 | try: 41 | exc = context['exception'] 42 | except KeyError: 43 | print(context['message']) 44 | return 45 | 46 | if isinstance(exc, ConnectionResetError): 47 | print("Got disconnect / connection reset - shutting down.") 48 | sys.exit(0) 49 | 50 | if isinstance(exc, OSError) and exc.errno == 9: 51 | print(exc) 52 | traceback.print_exc() 53 | return 54 | 55 | # traceback.print_exc() 56 | raise exc 57 | 58 | try: 59 | arguments = parser.parse_args() 60 | loop = asyncio.get_event_loop() 61 | loop.set_exception_handler(handler) 62 | run(loop, arguments) 63 | except KeyboardInterrupt: 64 | print("Got CtrlC, shutting down.") 65 | 66 | 67 | if __name__ == '__main__': 68 | print("STARTING") 69 | main() 70 | print("FINISHED") 71 | 72 | -------------------------------------------------------------------------------- /test_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/test_py/__init__.py -------------------------------------------------------------------------------- /test_py/convert/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/test_py/convert/__init__.py -------------------------------------------------------------------------------- /test_py/generate/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/test_py/generate/__init__.py -------------------------------------------------------------------------------- /test_py/generate/test_revolvebot.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from pyrevolve.revolve_bot import RevolveBot 4 | 5 | 6 | class TestRevolveBot(unittest.TestCase): 7 | """ 8 | Basic tests for RobolveBot body and brain generation 9 | """ 10 | def revolve_bot_equal(self, roba, robb): 11 | self._revolve_bot_body_equal(roba._body, robb._body) 12 | self._revolve_bot_brain_equal(roba._brain, robb._brain) 13 | 14 | def _revolve_bot_body_equal(self, bodya, bodyb): 15 | self.assertEqual(type(bodya), type(bodyb)) 16 | 17 | def _revolve_bot_brain_equal(self, braina, brainb): 18 | self.assertEqual(type(braina), type(brainb)) 19 | 20 | def _proto_test(self, filename): 21 | 22 | revolve_bot = RevolveBot() 23 | revolve_bot.load_file( 24 | path=filename, 25 | conf_type='yaml' 26 | ) 27 | revolve_bot.save_file( 28 | path='/tmp/revolve_bot.yaml', 29 | conf_type='yaml' 30 | ) 31 | 32 | revolve_bot2 = RevolveBot() 33 | revolve_bot2.load_file( 34 | path='/tmp/revolve_bot.yaml', 35 | conf_type='yaml' 36 | ) 37 | 38 | self.revolve_bot_equal(revolve_bot, revolve_bot2) 39 | 40 | def test_load_save_yaml(self): 41 | """ 42 | We load a YAML file and save it 43 | """ 44 | self._proto_test('experiments/examples/yaml/simple_robot.yaml') 45 | self._proto_test('experiments/examples/yaml/spider.yaml') 46 | self._proto_test('experiments/examples/yaml/gecko.yaml') 47 | self._proto_test('experiments/examples/yaml/snake.yaml') 48 | -------------------------------------------------------------------------------- /test_py/plasticonding/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/test_py/plasticonding/__init__.py -------------------------------------------------------------------------------- /test_py/plasticonding/genotype_176.txt: -------------------------------------------------------------------------------- 1 | AJ1 brainmoveFTS_1.000000|1.000000 brainloop_0.416317 addl B moveb mover brainmoveFTS_1.000000|1.000000 brainampperturb_0.055473 addr AJ2_-0.233663|2.183440|3.291942|4.403511 movel movef 2 | AJ2 brainmoveFTS_1.000000|1.000000 brainperperturb_0.802147 addr AJ2_-0.451750|2.034820|7.606433|6.574293 mover movel brainmoveFTS_1.000000|1.000000 brainoffperturb_0.344535 addf AJ2_0.395370|5.811625|3.816685|8.139162 moveb moveb brainmoveFTS_4.000000|2.000000 brainloop_-0.991706 addl B mover movef 3 | B brainmoveTTS_2.000000|1.000000 brainloop_0.802269 addr B movef movel brainmoveFTS_2.000000|1.000000 brainedge_-0.381986 addf ST_0.780739 movel movef brainmoveFTS_3.000000|1.000000 brainedge_-0.077519 addf AJ2_-0.547769|6.412029|8.911399|9.774277 moveb mover 4 | C C brainmoveFTS_2.000000|3.000000 brainoffperturb_-1.051822 addf AJ1_-0.571851|1.839181|4.364775|1.276114 movef moveb brainmoveTTS_3.000000|1.000000 brainperperturb_-0.759291 addf AJ1_0.697915|8.120513|3.796636|6.375849 movel mover 5 | ST brainmoveTTS_1.000000|1.000000 brainperperturb_-1.003928 addr AJ1_-0.091552|9.117664|6.366138|9.261707 mover mover 6 | -------------------------------------------------------------------------------- /test_py/plasticonding/genotype_180.txt: -------------------------------------------------------------------------------- 1 | AJ1 brainmoveTTS_1.000000|1.000000 brainperperturb_-0.393406 addl AJ1_-0.393280|1.988274|1.096442|9.835634 movel movel brainmoveFTS_1.000000|1.000000 brainedge_-0.101278 addf B mover movel brainmoveFTS_2.000000|1.000000 brainedge_-0.979029 addf B moveb moveb 2 | AJ2 brainmoveTTS_1.000000|1.000000 brainperperturb_-1.448493 addl AJ2_-0.655487|8.519603|9.441968|6.030500 addl moveb movel 3 | B brainmoveTTS_1.000000|1.000000 brainperperturb_-0.192989 addf AJ1_0.814148|7.880737|6.556612|5.823772 movef mover brainmoveFTS_1.000000|1.000000 brainedge_0.984046 addl B movel movel 4 | C C brainmoveFTS_1.000000|2.000000 brainperturb_-1.116291 addr B mover movef brainmoveFTS_1.000000|1.000000 brainedge_-0.413422 ST_-0.482408 addr ST_0.425158 moveb movef brainmoveTTS_1.000000|1.000000 brainampperturb_-1.216602 addf ST_0.351256 moveb moveb 5 | ST brainmoveFTS_1.000000|1.000000 brainampperturb_0.930132 addr AJ1_0.706664|3.625587|6.327397|6.959880 mover mover brainmoveFTS_1.000000|1.000000 brainoffperturb_0.884765 addf AJ1_-0.945926|2.032773|3.248924|1.285235 movef mover 6 | -------------------------------------------------------------------------------- /test_py/spec/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ci-group/revolve/a8f181495d3ba7664b34f6f6bbfb4a611aebde33/test_py/spec/__init__.py -------------------------------------------------------------------------------- /test_py/util/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | __author__ = 'elte' 4 | -------------------------------------------------------------------------------- /test_robots.py: -------------------------------------------------------------------------------- 1 | import asyncio 2 | 3 | from pyrevolve import parser 4 | from pyrevolve.revolve_bot import RevolveBot 5 | from pyrevolve.tol.manage import World 6 | 7 | 8 | async def run(): 9 | 10 | settings = parser.parse_args() 11 | yaml_file = 'experiments/'+ settings.experiment_name +'/data_fullevolution/phenotypes/'+'phenotype_'+settings.test_robot+'.yaml' 12 | 13 | r = RevolveBot(_id=settings.test_robot) 14 | r.load_file(yaml_file, conf_type='yaml') 15 | #r.save_file('experiments/'+ settings +'/data_fullevolution/phenotype_35.sdf.xml', conf_type='sdf') 16 | #r.render_body('experiments/'+ settings +'/data_fullevolution/phenotypes/phenotype_35.body.png') 17 | 18 | connection = await World.create(settings, world_address=("127.0.0.1", settings.port_start)) 19 | await connection.insert_robot(r) 20 | 21 | 22 | def main(): 23 | import traceback 24 | 25 | def handler(_loop, context): 26 | try: 27 | exc = context['exception'] 28 | except KeyError: 29 | print(context['message']) 30 | return 31 | traceback.print_exc() 32 | raise exc 33 | 34 | try: 35 | loop = asyncio.get_event_loop() 36 | loop.set_exception_handler(handler) 37 | loop.run_until_complete(run()) 38 | except KeyboardInterrupt: 39 | print("Got CtrlC, shutting down.") 40 | 41 | 42 | if __name__ == '__main__': 43 | print("STARTING") 44 | main() 45 | print("FINISHED") 46 | -------------------------------------------------------------------------------- /thirdparty/PIGPIO/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.0) 2 | 3 | message("Building PIGPIO") 4 | 5 | set(THREADS_PREFER_PTHREAD_FLAG ON) 6 | find_package(Threads REQUIRED) 7 | 8 | add_library(pigpio SHARED 9 | pigpio.c 10 | command.c) 11 | target_link_libraries(pigpio 12 | PUBLIC Threads::Threads) 13 | 14 | add_library(pigpio_if SHARED 15 | pigpiod_if.c 16 | command.c) 17 | target_link_libraries(pigpio_if 18 | PUBLIC Threads::Threads) 19 | 20 | add_library(pigpio_if2 SHARED 21 | pigpiod_if2.c 22 | command.c) 23 | target_link_libraries(pigpio_if2 24 | PUBLIC Threads::Threads) 25 | 26 | add_executable(x_pigpio 27 | x_pigpio.c) 28 | target_link_libraries(x_pigpio PUBLIC pigpio) 29 | 30 | add_executable(x_pigpiod_if 31 | x_pigpiod_if.c) 32 | target_link_libraries(x_pigpiod_if 33 | PUBLIC pigpio_if) 34 | 35 | add_executable(x_pigpiod_if2 36 | x_pigpiod_if2.c) 37 | target_link_libraries(x_pigpiod_if2 38 | PUBLIC pigpio_if2) 39 | 40 | add_executable(pigpiod 41 | pigpiod.c) 42 | target_link_libraries(pigpiod PUBLIC pigpio) 43 | 44 | add_executable(pigs 45 | pigs.c 46 | command.c) 47 | 48 | add_executable(pig2vcd 49 | pig2vcd.c) 50 | 51 | set(PIGPIO_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR} PARENT_SCOPE) 52 | 53 | install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 54 | PUBLIC_HEADER DESTINATION include 55 | FILES_MATCHING PATTERN "pi*.h") 56 | 57 | install(TARGETS pigpio pigpio_if pigpio_if2 pig2vcd pigpiod pigs 58 | RUNTIME DESTINATION bin 59 | LIBRARY DESTINATION lib) 60 | 61 | install(FILES pigpio.service 62 | DESTINATION /etc/systemd/system/) 63 | 64 | enable_testing() 65 | add_test(NAME pigpio 66 | COMMAND x_pigpio) 67 | add_test(NAME pigpiod_if 68 | COMMAND x_pigpiod_if) 69 | add_test(NAME pigpiod_if2 70 | COMMAND x_pigpiod_if2) -------------------------------------------------------------------------------- /thirdparty/PIGPIO/UNLICENCE: -------------------------------------------------------------------------------- 1 | This is free and unencumbered software released into the public domain. 2 | 3 | Anyone is free to copy, modify, publish, use, compile, sell, or 4 | distribute this software, either in source code form or as a compiled 5 | binary, for any purpose, commercial or non-commercial, and by any 6 | means. 7 | 8 | In jurisdictions that recognize copyright laws, the author or authors 9 | of this software dedicate any and all copyright interest in the 10 | software to the public domain. We make this dedication for the benefit 11 | of the public at large and to the detriment of our heirs and 12 | successors. We intend this dedication to be an overt act of 13 | relinquishment in perpetuity of all present and future rights to this 14 | software under copyright law. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 19 | IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR 20 | OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 21 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 22 | OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | For more information, please refer to 25 | 26 | -------------------------------------------------------------------------------- /thirdparty/PIGPIO/custom.cext: -------------------------------------------------------------------------------- 1 | /* 2 | This version is for pigpio version 26+ 3 | 4 | If you want customised functions replace this file with your own 5 | definitions for gpioCustom1 and gpioCustom2. 6 | */ 7 | 8 | #include "pigpio.h" 9 | 10 | int gpioCustom1(unsigned arg1, unsigned arg2, char *argx, unsigned count) 11 | { 12 | int i; 13 | unsigned max; 14 | 15 | DBG(DBG_USER, "arg1=%d arg2=%d count=%d [%s]", 16 | arg1, arg2, count, myBuf2Str(count, argx)); 17 | 18 | CHECK_INITED; 19 | 20 | /* for dummy just return max parameter */ 21 | 22 | if (arg1 > arg2) max = arg1; else max = arg2; 23 | 24 | for (i=0; i max) max = argx[i]; 25 | 26 | return max; 27 | } 28 | 29 | 30 | int gpioCustom2(unsigned arg1, char *argx, unsigned count, 31 | char *retBuf, unsigned retMax) 32 | { 33 | int i, j, t; 34 | 35 | DBG(DBG_USER, "arg1=%d count=%d [%s] retMax=%d", 36 | arg1, count, myBuf2Str(count, argx), retMax); 37 | 38 | CHECK_INITED; 39 | 40 | /* for dummy just return argx reversed */ 41 | 42 | if (count > retMax) count = retMax; 43 | 44 | for (i=0, j=count-1; i<=j; i++, j--) 45 | { 46 | /* t used as argx and retBuf may be the same buffer */ 47 | t = argx[i]; 48 | retBuf[i] = argx[j]; 49 | retBuf[j] = t; 50 | } 51 | 52 | return count; 53 | } 54 | 55 | -------------------------------------------------------------------------------- /thirdparty/PIGPIO/pigpio.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=PIGPIO daemon 3 | After=network.target auditd.service 4 | 5 | [Service] 6 | ExecStart=/usr/local/bin/pigpiod -g 7 | #KillMode=process 8 | Restart=on-failure 9 | #RestartPreventExitStatus=255 10 | Type=exec 11 | CPUSchedulingPolicy=fifo 12 | CPUSchedulingPriority=20 13 | 14 | [Install] 15 | WantedBy=multi-user.target 16 | Alias=PIGPIO.service 17 | -------------------------------------------------------------------------------- /thirdparty/PIGPIO/pigs.h: -------------------------------------------------------------------------------- 1 | /* 2 | This is free and unencumbered software released into the public domain. 3 | 4 | Anyone is free to copy, modify, publish, use, compile, sell, or 5 | distribute this software, either in source code form or as a compiled 6 | binary, for any purpose, commercial or non-commercial, and by any 7 | means. 8 | 9 | In jurisdictions that recognize copyright laws, the author or authors 10 | of this software dedicate any and all copyright interest in the 11 | software to the public domain. We make this dedication for the benefit 12 | of the public at large and to the detriment of our heirs and 13 | successors. We intend this dedication to be an overt act of 14 | relinquishment in perpetuity of all present and future rights to this 15 | software under copyright law. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 18 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 19 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 20 | IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR 21 | OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 22 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 23 | OTHER DEALINGS IN THE SOFTWARE. 24 | 25 | For more information, please refer to 26 | */ 27 | 28 | /* 29 | This version is for pigpio version 67+ 30 | */ 31 | 32 | #ifndef PIGS_H 33 | #define PIGS_H 34 | 35 | #define PIGS_OK 0 36 | #define PIGS_CONNECT_ERR 255 37 | #define PIGS_OPTION_ERR 254 38 | #define PIGS_SCRIPT_ERR 253 39 | 40 | #endif 41 | 42 | -------------------------------------------------------------------------------- /thirdparty/PIGPIO/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | 5 | setup(name='pigpio', 6 | version='1.42', 7 | author='joan', 8 | author_email='joan@abyz.me.uk', 9 | maintainer='joan', 10 | maintainer_email='joan@abyz.me.uk', 11 | url='http://abyz.me.uk/rpi/pigpio/python.html', 12 | description='Raspberry Pi GPIO module', 13 | long_description='Raspberry Pi Python module to access the pigpio daemon', 14 | download_url='http://abyz.me.uk/rpi/pigpio/pigpio.zip', 15 | license='unlicense.org', 16 | py_modules=['pigpio'], 17 | keywords=['raspberrypi', 'gpio',], 18 | classifiers=[ 19 | "Programming Language :: Python :: 2", 20 | "Programming Language :: Python :: 3", 21 | ] 22 | ) 23 | 24 | -------------------------------------------------------------------------------- /tools/analyzer/analyzer-world.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /tools/analyzer/run-analyzer: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) 3 | BASE=$DIR/../../build 4 | ANALYZER=$BASE/body-analyzer 5 | ANALYZER_TOOL=1 GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:$BASE/lib $ANALYZER $DIR/analyzer-world.world 6 | -------------------------------------------------------------------------------- /tools/cmake_eclipse: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | mkdir -p ../build 3 | cd ../build 4 | cmake -DCMAKE_BUILD_TYPE=Debug -G"Eclipse CDT4 - Unix Makefiles" ../cpp/ 5 | -------------------------------------------------------------------------------- /tools/models/rg_robot/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | ToL Sample robot 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Elte Hupkes 9 | elte@hupkes.org 10 | 11 | 12 | 13 | Mostly a container of meshes for path convenience. 14 | 15 | 16 | -------------------------------------------------------------------------------- /tools/msg_check.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import os 4 | import re 5 | import sys 6 | 7 | # Get current working directory 8 | cwd = os.getcwd() 9 | 10 | # If running from in the tools directory, then add a "/.." 11 | if cwd.endswith("/tools"): 12 | cwd = cwd + "/.." 13 | 14 | # XML always need a results tag, even when there is no errors 15 | if len(sys.argv) > 1 and sys.argv[1] == "xml": 16 | sys.stderr.write('''\n''') 17 | sys.stderr.write('''\n''') 18 | 19 | # Iterate over all the .proto files 20 | for filename in os.listdir(cwd + "/cpprevolve/revolve/msgs"): 21 | if filename.endswith('.proto'): 22 | with open(cwd + "/cpprevolve/revolve/msgs/" + filename, 'r') as f: 23 | for line in f.readlines(): 24 | # Get the "/// \interface (*)" 25 | iface_match = re.search('^///\s\\\\interface\s+.*$', line) 26 | if iface_match: 27 | iface = line[iface_match.start()+15:iface_match.end()].strip() 28 | 29 | # Get the "message (*)" 30 | msg_match = re.search('^message\s+.*$', line) 31 | if msg_match: 32 | msg = line[msg_match.start()+8:msg_match.end()].strip() 33 | 34 | # Make sure the two match 35 | if iface != msg: 36 | msg = "Mismatch between \interface and message: %s" % filename 37 | if len(sys.argv) > 1 and sys.argv[1] == "xml": 38 | sys.stderr.write('''\n''' % (filename, msg)) 39 | else: 40 | sys.stderr.write(msg + "\n") 41 | 42 | if len(sys.argv) > 1 and sys.argv[1] == "xml": 43 | sys.stderr.write('''\n''') 44 | -------------------------------------------------------------------------------- /tools/proto2python.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | PROTO_FOLDER='../cpprevolve/revolve/gazebo/msgs' 6 | GAZEBO_PROTO_FOLDER='/home/matteo/Tools/gazebo/include/gazebo-10/gazebo/msgs/proto/' 7 | PY_PROTOBUF_FOLDER='../pyrevolve/spec/msgs/' 8 | 9 | # Generate Python protobuf files 10 | protoc -I ${PROTO_FOLDER} \ 11 | -I ${GAZEBO_PROTO_FOLDER} \ 12 | --python_out=${PY_PROTOBUF_FOLDER} \ 13 | ${PROTO_FOLDER}/*.proto 14 | 15 | # Correctly reference imported Gazebo messages 16 | sed -i -E 's/import vector3d_pb2/from pygazebo.msg import vector3d_pb2/g' ${PY_PROTOBUF_FOLDER}/*.py 17 | sed -i -E 's/import pose_pb2/from pygazebo.msg import pose_pb2/g' ${PY_PROTOBUF_FOLDER}/*.py 18 | sed -i -E 's/import time_pb2/from pygazebo.msg import time_pb2/g' ${PY_PROTOBUF_FOLDER}/*.py 19 | sed -i -E 's/import model_pb2/from pygazebo.msg import model_pb2/g' ${PY_PROTOBUF_FOLDER}/*.py 20 | 21 | sed -i -E 's/import body_pb2/from . import body_pb2/g' ${PY_PROTOBUF_FOLDER}/*.py 22 | sed -i -E 's/import parameter_pb2/from . import parameter_pb2/g' ${PY_PROTOBUF_FOLDER}/*.py 23 | sed -i -E 's/import neural_net_pb2/from . import neural_net_pb2/g' ${PY_PROTOBUF_FOLDER}/*.py 24 | 25 | #it wont write the improt correctly. gotta change 'import' to 'from . import' 26 | #in some cases 'from pygazebo.msg import' -------------------------------------------------------------------------------- /worlds/benchmark.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 0.0 13 | 14 | 15 | 16 | 0.1 17 | 10e-6 18 | 19 | 20 | 100 21 | 1e-8 22 | 23 | 24 | quick 25 | 26 | 27 | 28 | 29 | 30 | 31 | model://sun 32 | 33 | 34 | model://tol_ground 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /worlds/benchmark2.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.005 7 | 8 | 9 | 0.0 10 | 11 | 12 | 13 | 0.1 14 | 10e-6 15 | 16 | 17 | 100 18 | 1e-8 19 | 20 | 21 | world 22 | 23 | 24 | 25 | 26 | 27 | 28 | model://sun 29 | 30 | 31 | model://tol_ground 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /worlds/demo.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 10 | 11 | 1.0 12 | 0.001 13 | 14 | 15 | 16 | 10e-6 17 | 0.1 18 | 19 | 20 | 21 | 22 | 23 | 24 | model://sun 25 | 26 | 27 | model://tol_ground 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /worlds/idemo.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 0.5 18 | 10e-6 19 | 20 | 21 | 22 | 23 | 24 | 25 | model://sun 26 | 27 | 28 | model://tol_ground 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /worlds/offline-evolve.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 0.0 13 | 14 | 15 | 16 | 0.1 17 | 10e-6 18 | 19 | 20 | 100 21 | 1e-8 22 | 23 | 24 | quick 25 | 26 | 27 | 28 | 29 | 30 | 31 | model://sun 32 | 33 | 34 | model://tol_ground 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /worlds/online-evolve.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 0.0 13 | 14 | 15 | 16 | 0.1 17 | 10e-6 18 | 19 | 20 | 100 21 | 1e-8 22 | 23 | 24 | quick 25 | 26 | 27 | 28 | 29 | 30 | 31 | model://sun 32 | 33 | 34 | model://tol_ground 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /worlds/plane.ensure_contacts.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 10 | 0.005 11 | 12 | 13 | 2000 14 | 15 | 16 | 17 | 18 | 0.1 19 | 10e-6 20 | 21 | 22 | 100 23 | 1e-8 24 | 25 | 26 | world 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | model://sun 36 | 37 | 38 | model://tol_ground 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /worlds/plane.realtime.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 10 | 0.001 11 | 12 | 13 | 800 14 | 15 | 16 | 17 | 18 | 0.1 19 | 10e-6 20 | 21 | 22 | 100 23 | 1e-8 24 | 25 | 26 | world 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | model://sun 36 | 37 | 38 | model://tol_ground 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /worlds/plane.unmanaged.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 10 | 0.02 11 | 12 | 13 | 0.0 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0.1 22 | 10e-6 23 | 24 | 25 | 100 26 | 1e-8 27 | 28 | 29 | world 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | model://sun 39 | 40 | 41 | model://tol_ground 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /worlds/plane.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 10 | 0.005 11 | 12 | 13 | 0 14 | 15 | 16 | 17 | 18 | 0.1 19 | 10e-6 20 | 21 | 22 | 100 23 | 1e-8 24 | 25 | 26 | world 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | model://sun 36 | 37 | 38 | model://tol_ground 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /worlds/tilted10.realtime.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 1000 13 | 14 | 15 | 16 | 17 | 0.1 18 | 10e-6 19 | 20 | 21 | 100 22 | 1e-8 23 | 24 | 25 | quick 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | model://sun 34 | 35 | 36 | model://tilted10 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /worlds/tilted10.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 0 13 | 14 | 15 | 16 | 0.1 17 | 10e-6 18 | 19 | 20 | 100 21 | 1e-8 22 | 23 | 24 | quick 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | model://sun 33 | 34 | 35 | model://tilted10 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /worlds/tilted15.realtime.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 1000 13 | 14 | 15 | 16 | 17 | 0.1 18 | 10e-6 19 | 20 | 21 | 100 22 | 1e-8 23 | 24 | 25 | quick 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | model://sun 34 | 35 | 36 | model://tilted15 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /worlds/tilted15.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 0 13 | 14 | 15 | 16 | 0.1 17 | 10e-6 18 | 19 | 20 | 100 21 | 1e-8 22 | 23 | 24 | quick 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | model://sun 33 | 34 | 35 | model://tilted15 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /worlds/tilted5.realtime.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 1000 13 | 14 | 15 | 16 | 0.1 17 | 10e-6 18 | 19 | 20 | 100 21 | 1e-8 22 | 23 | 24 | quick 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | model://sun 33 | 34 | 35 | model://tilted5 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /worlds/tilted5.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 11 | 12 | 0 13 | 14 | 15 | 16 | 0.1 17 | 10e-6 18 | 19 | 20 | 100 21 | 1e-8 22 | 23 | 24 | quick 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | model://sun 33 | 34 | 35 | model://tilted5 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /worlds/viewer.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 0.003 10 | 333 11 | 12 | 13 | 14 | 0.1 15 | 10e-6 16 | 17 | 18 | 100 19 | 1e-8 20 | 21 | 22 | quick 23 | 24 | 25 | 26 | 27 | 28 | 29 | model://sun 30 | 31 | 32 | model://tol_ground 33 | 34 | 35 | model://test_bot 36 | 37 | 38 | 39 | 40 | 0.407182 -0.446632 1.00088 0 1.06364 2.56819 41 | orbit 42 | perspective 43 | 44 | 45 | 46 | 47 | 48 | 49 | --------------------------------------------------------------------------------