├── _config.yml ├── logo.png ├── conf ├── policies │ ├── pfsm │ │ ├── aggregation_policy.txt │ │ ├── policy_evolved_temp.txt │ │ ├── policy_observe.txt │ │ ├── aggregation_policy_evolved.txt │ │ ├── exploration_policy_evolved.txt │ │ ├── exploration_policy_random.txt │ │ └── aggregation_policy_benchmark.txt │ └── behavior_trees │ │ ├── behavior_tree_example.bt │ │ └── behavior_tree_evolved_aggregation.bt ├── environments │ ├── square.txt │ ├── funnel.txt │ └── rooms.txt ├── parameters.xsd └── parameters.xml ├── scripts ├── README.md ├── python │ ├── requirements.txt │ ├── tools │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ ├── __init__.cpython-36.pyc │ │ │ └── fileHandler.cpython-36.pyc │ │ ├── matrixOperations.py │ │ └── fileHandler.py │ ├── classes │ │ ├── __pycache__ │ │ │ ├── evolution.cpython-36.pyc │ │ │ └── swarmulator.cpython-36.pyc │ │ ├── swarmulator.py │ │ └── evolution.py │ ├── README.md │ └── evolution_example.py ├── timelog.sh ├── parallel_simulations.sh ├── fix_all_code_style.sh ├── record_and_convert.sh ├── run_n_simulations.sh ├── fix_code_style.sh ├── make_new_controller.sh └── make_new_agent.sh ├── sw ├── simulation │ ├── controllers │ │ ├── boid │ │ │ ├── README.md │ │ │ ├── boid.h │ │ │ └── boid.cpp │ │ ├── random_exploration │ │ │ ├── README.md │ │ │ ├── random_exploration.cpp │ │ │ └── random_exploration.h │ │ ├── behavior_tree │ │ │ ├── bt │ │ │ │ ├── btCommon.h │ │ │ │ ├── robots │ │ │ │ │ ├── robotActions.h │ │ │ │ │ ├── robotActions.cpp │ │ │ │ │ └── robotConditions.cpp │ │ │ │ ├── conditions │ │ │ │ │ ├── memory_set.h │ │ │ │ │ ├── greater_than.h │ │ │ │ │ └── less_than.h │ │ │ │ ├── node.cpp │ │ │ │ ├── blackboard.cpp │ │ │ │ ├── btFile.h │ │ │ │ ├── blackboard.h │ │ │ │ ├── actions │ │ │ │ │ └── wheelSpeed.h │ │ │ │ ├── node.h │ │ │ │ ├── condition.h │ │ │ │ ├── composites.h │ │ │ │ └── btFile.cpp │ │ │ ├── behavior_tree.h │ │ │ ├── behavior_tree_wheeled.h │ │ │ ├── behavior_tree_wheeled.cpp │ │ │ ├── README.md │ │ │ └── behavior_tree.cpp │ │ ├── pytorch │ │ │ ├── pytorch.cpp │ │ │ ├── pytorch.h │ │ │ └── README.md │ │ ├── pattern_formation │ │ │ ├── lattice.h │ │ │ ├── README.md │ │ │ ├── lattice.cpp │ │ │ ├── pattern_formation.h │ │ │ ├── pattern_formation.cpp │ │ │ ├── lattice_basic.h │ │ │ └── lattice_basic.cpp │ │ ├── forage │ │ │ ├── README.md │ │ │ ├── forage.h │ │ │ └── forage.cpp │ │ ├── leader_follower │ │ │ ├── ekf_state_estimator.h │ │ │ ├── README.md │ │ │ ├── leader_follower.h │ │ │ └── ekf_state_estimator.cpp │ │ └── aggregation │ │ │ ├── README.md │ │ │ ├── aggregation.h │ │ │ └── aggregation.cpp │ ├── agents │ │ ├── wheeled │ │ │ ├── README.md │ │ │ ├── wheeled.h │ │ │ └── wheeled.cpp │ │ ├── particle │ │ │ ├── README.md │ │ │ ├── particle.h │ │ │ └── particle.cpp │ │ ├── particle_oriented │ │ │ ├── README.md │ │ │ ├── particle_oriented.h │ │ │ └── particle_oriented.cpp │ │ └── particle_oriented_xy │ │ │ ├── README.md │ │ │ ├── particle_oriented_xy.h │ │ │ └── particle_oriented_xy.cpp │ ├── agent.cpp │ ├── fifo.cpp │ ├── fifo.h │ ├── sensors │ │ ├── template_calculator.h │ │ ├── template_calculator.cpp │ │ └── omniscient_observer.cpp │ ├── agent.h │ ├── environment.h │ ├── fitness_functions.h │ ├── agent_thread.h │ ├── environment.cpp │ ├── controller.h │ ├── controller.cpp │ └── simulation_thread.h ├── settings.h ├── animation │ ├── drawingparams.h │ ├── terminalinfo.cpp │ ├── terminalinfo.h │ ├── animation_thread.h │ ├── draw.h │ └── draw.cpp ├── main.h ├── logger │ ├── txtwrite.cpp │ ├── txtwrite.h │ └── logger_thread.h ├── math │ ├── randomgenerator.cpp │ ├── graph.h │ ├── randomgenerator.h │ ├── graph.cpp │ └── trigonometry.h └── main.cpp ├── .gitignore ├── .vscode └── launch.json └── makefile /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-cayman -------------------------------------------------------------------------------- /logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/coppolam/swarmulator/HEAD/logo.png -------------------------------------------------------------------------------- /conf/policies/pfsm/aggregation_policy.txt: -------------------------------------------------------------------------------- 1 | 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 2 | -------------------------------------------------------------------------------- /scripts/README.md: -------------------------------------------------------------------------------- 1 | This folders is to group the bash scripts for Swarmulator 2 | -------------------------------------------------------------------------------- /scripts/python/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | scipy 3 | networkx 4 | deap 5 | tqdm 6 | matplotlib 7 | -------------------------------------------------------------------------------- /conf/policies/pfsm/policy_evolved_temp.txt: -------------------------------------------------------------------------------- 1 | 0.675 2 | 0.666 3 | 0.986 4 | 0.484 5 | 0.781 6 | 0.547 7 | 0.421 8 | 0.506 -------------------------------------------------------------------------------- /conf/environments/square.txt: -------------------------------------------------------------------------------- 1 | -20.0 20.0 20.0 20.0 2 | 20.0 20.0 20.0 -20.0 3 | -20.0 -20.0 -20.0 20.0 4 | 20.0 -20.0 -20.0 -20.0 -------------------------------------------------------------------------------- /scripts/python/tools/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Collection of tools 4 | @author: Mario Coppola, 2020 5 | """ -------------------------------------------------------------------------------- /conf/environments/funnel.txt: -------------------------------------------------------------------------------- 1 | 0.0 -2.0 8.0 -8.0 2 | 8.0 -8.0 8.0 8.0 3 | 8.0 8.0 0.0 2.0 4 | 0.0 2.0 -8.0 8.0 5 | -8.0 8.0 -8.0 -8.0 6 | -8.0 -8.0 0.0 -2.0 -------------------------------------------------------------------------------- /scripts/timelog.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cd .. 3 | cd logs 4 | mkdir evaluation_time 5 | cd .. 6 | for i in {2 5 10 20 50 100} 7 | do 8 | ./swarmulator $i 9 | done -------------------------------------------------------------------------------- /scripts/python/tools/__pycache__/__init__.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/coppolam/swarmulator/HEAD/scripts/python/tools/__pycache__/__init__.cpython-36.pyc -------------------------------------------------------------------------------- /scripts/python/classes/__pycache__/evolution.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/coppolam/swarmulator/HEAD/scripts/python/classes/__pycache__/evolution.cpython-36.pyc -------------------------------------------------------------------------------- /scripts/python/classes/__pycache__/swarmulator.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/coppolam/swarmulator/HEAD/scripts/python/classes/__pycache__/swarmulator.cpython-36.pyc -------------------------------------------------------------------------------- /scripts/python/tools/__pycache__/fileHandler.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/coppolam/swarmulator/HEAD/scripts/python/tools/__pycache__/fileHandler.cpython-36.pyc -------------------------------------------------------------------------------- /sw/simulation/controllers/boid/README.md: -------------------------------------------------------------------------------- 1 | # boid 2 | 3 | This simple controller makes the agents move with boid like dynamics. 4 | 5 | Designed to work with AGENT=particle_oriented. -------------------------------------------------------------------------------- /conf/policies/pfsm/policy_observe.txt: -------------------------------------------------------------------------------- 1 | -0.000 2 | 0.000 3 | 0.000 4 | 1.000 5 | 1.000 6 | 1.000 7 | 0.500 8 | 1.000 9 | 1.000 10 | 1.000 11 | 1.000 12 | 1.000 13 | 1.000 14 | 0.000 15 | 0.000 16 | 1.000 -------------------------------------------------------------------------------- /sw/simulation/agents/wheeled/README.md: -------------------------------------------------------------------------------- 1 | # wheeled 2 | 3 | It uses the dynamics of wheeled robots. 4 | 5 | It expects the inputs to be: 6 | 7 | * rotation speed of left wheel 8 | 9 | * rotation speed of right wheel -------------------------------------------------------------------------------- /sw/simulation/controllers/random_exploration/README.md: -------------------------------------------------------------------------------- 1 | # exploration 2 | This controller is designed to explore closed spaces. The current implementation just moves randomly. 3 | 4 | Designed to work with particle_oriented. -------------------------------------------------------------------------------- /sw/simulation/agents/particle/README.md: -------------------------------------------------------------------------------- 1 | Basic accelerated particle with kinematic equations. 2 | 3 | Is represented as a circle. 4 | 5 | It expects the inputs to be the desired velocities, in the global frame, along x and y. -------------------------------------------------------------------------------- /conf/policies/behavior_trees/behavior_tree_example.bt: -------------------------------------------------------------------------------- 1 | compositeselectorselector 2 | conditiongreater_than0,0.48robot 3 | actionwheelSpeed0.1066,0.1375robot -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/btCommon.h: -------------------------------------------------------------------------------- 1 | #ifndef BTCOMMON_H 2 | #define BTCOMMON_H 3 | 4 | #define __BEHAVIOURTREE 5 | 6 | #include "node.h" 7 | #include "composites.h" 8 | #include "condition.h" 9 | 10 | #include "robots/robotActions.h" 11 | 12 | #endif // BTCOMMON_H 13 | -------------------------------------------------------------------------------- /scripts/parallel_simulations.sh: -------------------------------------------------------------------------------- 1 | # Bash script to run parallel batches of simulations (it launches a batch every five seconds) 2 | # $1: How many batches 3 | # $2: How many simulations per batch 4 | # $3: How many agents in the simulation 5 | 6 | for (( i = 1; i <= $1; i++ )); do 7 | ./run_n_simulations.sh $2 $3 & sleep 5 8 | done 9 | -------------------------------------------------------------------------------- /sw/simulation/controllers/boid/boid.h: -------------------------------------------------------------------------------- 1 | #ifndef BOID_H 2 | #define BOID_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "controller.h" 8 | 9 | #define COMMAND_LOCAL 1 10 | 11 | class boid: public Controller 12 | { 13 | public: 14 | boid(); 15 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 16 | virtual void animation(const uint16_t ID); 17 | }; 18 | 19 | #endif /*BOID_H*/ 20 | -------------------------------------------------------------------------------- /sw/simulation/controllers/pytorch/pytorch.cpp: -------------------------------------------------------------------------------- 1 | #include "pytorch.h" 2 | #include "draw.h" 3 | #include 4 | 5 | void pytorch::get_velocity_command(const uint16_t ID, float &v_x, float &v_y) 6 | { 7 | // Just an example of a tensor. Have fun. Use the instructions in the README to build. 8 | torch::Tensor tensor = torch::rand({2, 3}); 9 | std::cout << tensor << std::endl; 10 | } 11 | 12 | void pytorch::animation(const uint16_t ID) 13 | { 14 | 15 | } -------------------------------------------------------------------------------- /sw/simulation/agent.cpp: -------------------------------------------------------------------------------- 1 | #include "agent.h" 2 | 3 | Agent::Agent() 4 | { 5 | activated = false; 6 | } 7 | 8 | float Agent::get_position(uint16_t dim) 9 | { 10 | if (dim < 3) { 11 | return state[dim]; 12 | } 13 | return 0; 14 | } 15 | 16 | float Agent::get_orientation() 17 | { 18 | return state[6]; 19 | } 20 | 21 | float Agent::get_state(const uint16_t i) 22 | { 23 | float noise = rg.gaussian_float(0.0, NOISE_R); 24 | return state[i] + noise; 25 | } 26 | -------------------------------------------------------------------------------- /sw/simulation/agents/particle_oriented/README.md: -------------------------------------------------------------------------------- 1 | # particle_oriented 2 | 3 | Basic accelerated particle with kinematic equations and orientation. 4 | 5 | It is represented as a triagle in the direction of the orientation. 6 | 7 | It expects the control inputs to be the velocity along x (local frame, i.e., forward) and the yaw rate. 8 | For the same agent that expects the inputs to be the velocities along x and along y, please check the modified controller `particle_oriented_xy`. -------------------------------------------------------------------------------- /sw/simulation/controllers/pattern_formation/lattice.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_LATTICE_H 2 | #define CONTROLLER_LATTICE_H 3 | 4 | #include "lattice_basic.h" 5 | #include "template_calculator.h" 6 | 7 | class lattice : public lattice_basic 8 | { 9 | public: 10 | lattice(); 11 | ~lattice() {}; 12 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 13 | virtual void animation(const uint16_t ID); 14 | }; 15 | 16 | #endif /*CONTROLLER_LATTICE_H*/ -------------------------------------------------------------------------------- /sw/simulation/agents/particle_oriented_xy/README.md: -------------------------------------------------------------------------------- 1 | # particle_oriented_xy 2 | 3 | Basic accelerated particle with kinematic equations and orientation. 4 | 5 | Is represented as a triagle in the direction of the orientation. 6 | 7 | Unlike the standard `particle_oriented` class, this one expects the control input to be velocities along x and y in the local body frame (i.e. forward and sideways), whereas the original expects the control inputs to be the velocity along x and the yaw rate. -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | conf/environments/rooms.txt 2 | swarmulator 3 | *logs*/ 4 | conf/*.txt 5 | conf/environments/random.txt 6 | build/ 7 | *.o 8 | state_action_matrix_free_test*.txt 9 | state_action_matrix*.txt 10 | .vscode/ 11 | Doxyfile 12 | dox/ 13 | html/ 14 | latex/ 15 | gen* 16 | neat 17 | statout 18 | build-swarmulator-Desktop-Debug/ 19 | sw/window/swarmulator.pro.user 20 | sw/window/swarmulator_window.h.autosave 21 | sw/build-swarmulator-Desktop-Debug 22 | script.sh 23 | docs/ 24 | logo_small.png 25 | mat/ 26 | conf/state_action_matrices -------------------------------------------------------------------------------- /scripts/fix_all_code_style.sh: -------------------------------------------------------------------------------- 1 | # Bash script to recursively fix all cpp and h files to the preferred formatting 2 | # It is best to run this before committing code 3 | 4 | # Get the script directory so we can call it from anywhere 5 | script_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 6 | dir=${script_dir%/*} # Go to root swarmulator folder 7 | 8 | # Run 9 | ${dir}/scripts/fix_code_style.sh "${dir}/sw/*.cpp"; 10 | ${dir}/scripts/fix_code_style.sh "${dir}/sw/*.h"; 11 | 12 | echo "astyle formatted the code." -------------------------------------------------------------------------------- /sw/settings.h: -------------------------------------------------------------------------------- 1 | #ifndef SETTINGS_H 2 | #define SETTINGS_H 3 | 4 | /** 5 | * Holds some higher level settings of the simulator 6 | * 7 | */ 8 | 9 | // Use SEQUENTIAL to launch robots one by one with the specified interval (in seconds), rather than all at once 10 | // #define SEQUENTIAL 5 11 | 12 | /** 13 | * Noise in relative sensing 14 | */ 15 | // TODO: Make runtime variable 16 | #define NOISE_R 0 // STDEV of noise in range, used by omniscient_observer 17 | #define NOISE_B 0 // STDEV of noise in bearing, used by omniscient_observer 18 | 19 | #endif /*SETTINGS_H*/ 20 | -------------------------------------------------------------------------------- /sw/simulation/controllers/forage/README.md: -------------------------------------------------------------------------------- 1 | # forage 2 | 3 | This controller makes the agents look for food in the environment and return it to a home beacon. 4 | 5 | The home beacon is placed at (0,0) by default. 6 | The environment has 100 food items placed in it by default. 7 | Please check the Environment class (`sw/simulation/environment.cpp` to modify these parameters). 8 | 9 | IMPORTANT NOTE: In the standar version, the food will only show up if the fitness function used is `food`, as indicated int he runtime parameters. 10 | 11 | Designed to work with AGENT=particle_oriented. -------------------------------------------------------------------------------- /scripts/record_and_convert.sh: -------------------------------------------------------------------------------- 1 | # Bash script to record a simulation 2 | # Requires the following package 3 | # - glc 4 | # - mencoder 5 | 6 | cd .. 7 | 8 | md=$(date +%Y-%m-%d-%T); 9 | glc-capture -o rec_$md.glc --fps=60 --disable-audio -s ./swarmulator $1 10 | 11 | glc-play rec_$md.glc -o - -y 1 | avconv -i - -video_size 300x300 -pix_fmt yuv444p \ 12 | -threads auto -pix_fmt yuv444p -preset ultrafast -qp 0 -y rec_$md.mp4 13 | 14 | # glc-play rec_$md.glc -y 1 -o - | mencoder -demuxer y4m - \ 15 | # -ovc lavc -lavcopts vcodec=mpeg4:vbitrate=10000 -o rec_$md.avi 16 | 17 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/robots/robotActions.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTACTIONS_H 2 | #define ROBOTACTIONS_H 3 | 4 | #include 5 | #include "node.h" 6 | #include 7 | 8 | namespace BT 9 | { 10 | /** 11 | * @brief Get the action node 12 | * 13 | * @param action 14 | * @param inputs 15 | * @return node* of the action 16 | */ 17 | node *getAction(std::string action, std::vector inputs); 18 | 19 | /** 20 | * @brief Get the Action object 21 | * 22 | * @param func 23 | * @return node* 24 | */ 25 | node *getAction(size_t func = (size_t) - 1); 26 | 27 | } 28 | #endif // ROBOTACTIONS_H 29 | -------------------------------------------------------------------------------- /sw/simulation/controllers/random_exploration/random_exploration.cpp: -------------------------------------------------------------------------------- 1 | #include "random_exploration.h" 2 | #include "draw.h" 3 | #include "auxiliary.h" 4 | #include 5 | 6 | #define SENSOR_MAX_RANGE 1.8 7 | 8 | random_exploration::random_exploration() : Controller() 9 | { 10 | set_max_sensor_range(SENSOR_MAX_RANGE); 11 | } 12 | 13 | void random_exploration::get_velocity_command(const uint16_t ID, float &v_x, float &psirate) 14 | { 15 | v_x = 0.5; 16 | psirate = rg.uniform_float(-M_PI, M_PI); 17 | wall_avoidance_turn(ID, v_x, psirate, SENSOR_MAX_RANGE); 18 | } 19 | 20 | void random_exploration::animation(const uint16_t ID) 21 | { 22 | draw d; 23 | d.circle_loop(SENSOR_MAX_RANGE); 24 | } -------------------------------------------------------------------------------- /sw/animation/drawingparams.h: -------------------------------------------------------------------------------- 1 | #ifndef DRAWINGPARAMS_H 2 | #define DRAWINGPARAMS_H 3 | 4 | /** 5 | * This file holds several parameters needed by the animation 6 | */ 7 | 8 | extern float center_x, center_y; // Visualized center of x and y with respect to ground truth 9 | extern float sx, sy; // Visualized transition 10 | extern float zoom, zoom_scale; // Current zoom value + scale of the zoom 11 | extern float pointer_x, pointer_y; // Where the cursor is located along x and y 12 | extern float xrat, yrat; // Window ratio along x and y 13 | extern bool paused; // Indicates if the simulation is paused 14 | extern bool mouse_motion; // Indicates if the mouse is moving 15 | 16 | #endif /* DRAWINGPARAMS_H */ 17 | -------------------------------------------------------------------------------- /sw/simulation/agents/particle/particle.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLE_H 2 | #define PARTICLE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "agent.h" 8 | 9 | /** 10 | * This child class of agent implements the dynamics of simple accelerated particles using a kinematic model 11 | */ 12 | class particle: public Agent 13 | { 14 | public: 15 | /** 16 | * Constructor 17 | */ 18 | particle(int i, std::vector state, float tstep); 19 | 20 | /** 21 | * State update implementation 22 | */ 23 | std::vector state_update(std::vector state); 24 | 25 | /** 26 | * Animation openGL implementation for visualization 27 | */ 28 | void animation(); 29 | }; 30 | 31 | #endif /*PARTICLE_H*/ -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/conditions/memory_set.h: -------------------------------------------------------------------------------- 1 | #ifndef MEMORY_SET_H 2 | #define MEMORY_SET_H 3 | 4 | #include "condition.h" 5 | 6 | namespace BT 7 | { 8 | 9 | class memory_set : public condition 10 | { 11 | public: 12 | memory_set(std::string vehicle_name, size_t param = MAX_SIZE) 13 | : condition(vehicle_name, "memory_set", param) 14 | { 15 | } 16 | memory_set(std::string vehicle_name, size_t param, double value) 17 | : condition(vehicle_name, "memory_set", param, value) 18 | { 19 | } 20 | 21 | BT_Status update(blackboard *BLKB) 22 | { 23 | if (BLKB->get("memory")) { 24 | return BH_SUCCESS; 25 | } else { 26 | return BH_FAILURE; 27 | } 28 | 29 | } 30 | }; 31 | 32 | } 33 | 34 | #endif // MEMORY_SET_H -------------------------------------------------------------------------------- /sw/simulation/controllers/forage/forage.h: -------------------------------------------------------------------------------- 1 | #ifndef FORAGE_H 2 | #define FORAGE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "controller.h" 8 | 9 | #define COMMAND_LOCAL 1 // Local frame 10 | 11 | class forage: public Controller 12 | { 13 | std::vector motion_p; // Probability of motion 14 | uint timer; // Timer measuring how long a robot has been moving 15 | bool explore; 16 | float vmean; 17 | float timelim; 18 | float v_x_ref, v_y_ref; 19 | uint st; 20 | bool holds_food, choose; 21 | float state; 22 | public: 23 | forage(); 24 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 25 | virtual void animation(const uint16_t ID); 26 | }; 27 | 28 | #endif /*FORAGE_H*/ 29 | -------------------------------------------------------------------------------- /sw/simulation/controllers/leader_follower/ekf_state_estimator.h: -------------------------------------------------------------------------------- 1 | #ifndef EKF_STATE_ESTIMATOR_H 2 | #define EKF_STATE_ESTIMATOR_H 3 | #include "controller.h" 4 | 5 | extern "C" { 6 | #include "discrete_ekf_no_north.h" 7 | } 8 | 9 | class ekf_state_estimator 10 | { 11 | // The omniscient observer is used to simulate sensing the other agents. 12 | OmniscientObserver o; 13 | bool initialized; 14 | uint16_t ID; 15 | uint16_t ID_tracked; 16 | float simtime_seconds_store; 17 | 18 | public: 19 | struct discrete_ekf_no_north ekf_rl; 20 | ekf_state_estimator(); 21 | ~ekf_state_estimator() {}; 22 | void init_ekf_filter(); 23 | void run_ekf_filter(); 24 | void run(uint16_t ID_in, uint16_t ID_tracked_in); 25 | }; 26 | 27 | #endif /*EKF_STATE_ESTIMATOR_H*/ 28 | -------------------------------------------------------------------------------- /sw/simulation/controllers/pytorch/pytorch.h: -------------------------------------------------------------------------------- 1 | #ifndef PYTORCH_H 2 | #define PYTORCH_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "controller.h" 8 | 9 | class pytorch: public Controller 10 | { 11 | public: 12 | /** 13 | * @brief Construct a new pytorch controller 14 | * 15 | */ 16 | pytorch(): Controller() {}; 17 | 18 | /** 19 | * @brief Get the velocity command object 20 | * 21 | * @param ID 22 | * @param v_x 23 | * @param v_y 24 | */ 25 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 26 | 27 | /** 28 | * @brief Animation 29 | * 30 | * @param ID 31 | */ 32 | virtual void animation(const uint16_t ID); 33 | }; 34 | 35 | #endif /*PYTORCH_H*/ 36 | -------------------------------------------------------------------------------- /sw/simulation/controllers/random_exploration/random_exploration.h: -------------------------------------------------------------------------------- 1 | #ifndef EXPLORATION_H 2 | #define EXPLORATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "controller.h" 8 | #include "randomgenerator.h" 9 | 10 | #define COMMAND_LOCAL 1 11 | 12 | /** 13 | * Basic exploration behavior which randomly moves in an environment while avoiding neighbors. 14 | * 15 | */ 16 | class random_exploration: public Controller 17 | { 18 | public: 19 | /** 20 | * @brief Construct a new random exploration object 21 | * 22 | */ 23 | random_exploration(); 24 | 25 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 26 | virtual void animation(const uint16_t ID); 27 | }; 28 | 29 | #endif /*EXPLORATION_H*/ 30 | -------------------------------------------------------------------------------- /sw/simulation/controllers/aggregation/README.md: -------------------------------------------------------------------------------- 1 | # aggregation 2 | 3 | ## Description 4 | This controller implements an aggregation behavior as was used in: 5 | 6 | **"The PageRank algorithm as a method to optimize swarm behavior through local analysis"**. 7 | *Mario Coppola, Jian Guo, Eberhard Gill, Guido de Croon, 2019.* 8 | Swarm Intelligence, December 2019, Volume 13, Issue 3–4, pp 277–319 9 | 10 | The paper is available open-access at this link: 11 | https://link.springer.com/article/10.1007/s11721-019-00172-z 12 | 13 | The behavior is similar to the one, though, due to the evolutions of Swarmulator since then, it may differ slightly. For the exact same behavior as in the paper, plese see the `SI_pagerank` branch of the repository. 14 | 15 | It is designed to work with AGENT=particle -------------------------------------------------------------------------------- /conf/policies/behavior_trees/behavior_tree_evolved_aggregation.bt: -------------------------------------------------------------------------------- 1 | compositeselectorselector 2 | conditiongreater_than0,2.9424robot 3 | compositesequencesequence 4 | actionwheelSpeed0.11,0.7robot 5 | conditiongreater_than0,0.08robot 6 | actionwheelSpeed0.1519,0.6513robot 7 | actionwheelSpeed0.0322,0.5275robot 8 | conditiongreater_than0,4.9144robot 9 | actionwheelSpeed0.6835,0.2576robot 10 | actionwheelSpeed0.5571,0.3778robot 11 | -------------------------------------------------------------------------------- /sw/simulation/agents/particle_oriented/particle_oriented.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLE_ORIENTED_H 2 | #define PARTICLE_ORIENTED_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "agent.h" 8 | 9 | /** 10 | * This child class of agent implements the dynamics of simple accelerated oriented vehicles using a kinematic model 11 | */ 12 | class particle_oriented: public Agent 13 | { 14 | public: 15 | /** 16 | * Constructor 17 | */ 18 | particle_oriented(int i, std::vector state, float tstep); 19 | 20 | /** 21 | * State update implementation 22 | */ 23 | std::vector state_update(std::vector state); 24 | 25 | /** 26 | * Animation openGL implementation for visualization 27 | */ 28 | void animation(); 29 | }; 30 | 31 | #endif /*PARTICLE_ORIENTED_H*/ -------------------------------------------------------------------------------- /sw/simulation/agents/particle_oriented_xy/particle_oriented_xy.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLE_ORIENTED_XY_H 2 | #define PARTICLE_ORIENTED_XY_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "agent.h" 8 | 9 | /** 10 | * This child class of agent implements the dynamics of simple accelerated oriented vehicles using a kinematic model 11 | */ 12 | class particle_oriented_xy: public Agent 13 | { 14 | public: 15 | /** 16 | * Constructor 17 | */ 18 | particle_oriented_xy(int i, std::vector state, float tstep); 19 | 20 | /** 21 | * State update implementation 22 | */ 23 | std::vector state_update(std::vector state); 24 | 25 | /** 26 | * Animation openGL implementation for visualization 27 | */ 28 | void animation(); 29 | }; 30 | 31 | #endif /*PARTICLE_ORIENTED_H*/ -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Behaviour.cpp 3 | * 4 | * Created on: 18 Feb 2015 5 | * Author: Kirk Scheper 6 | * Email: kirkscheper@gmail.com 7 | */ 8 | 9 | #include "node.h" 10 | #include 11 | 12 | namespace BT 13 | { 14 | 15 | // Evaluate node 16 | BT_Status node::tick(blackboard *BLKB) 17 | { 18 | BT_Status returnStatus; 19 | // If not initialised, initialize 20 | if (m_eStatus == BH_INVALID) { 21 | onInitialise(); 22 | } 23 | 24 | // Run node behaviour 25 | m_eStatus = update(BLKB); 26 | returnStatus = m_eStatus; 27 | 28 | // If completed, run terminate code and reinitialise 29 | if (m_eStatus != BH_RUNNING) { 30 | onTerminate(m_eStatus); 31 | m_eStatus = BH_INVALID; 32 | } 33 | 34 | tick_counter++; 35 | 36 | return returnStatus; 37 | } 38 | 39 | } 40 | -------------------------------------------------------------------------------- /conf/policies/pfsm/aggregation_policy_evolved.txt: -------------------------------------------------------------------------------- 1 | 0.100 0.286 0.100 0.000 0.100 0.100 0.100 0.215 2 | 0.116 0.127 0.117 0.117 0.117 0.141 0.000 0.266 3 | 0.141 0.328 0.167 0.067 0.002 0.153 0.000 0.141 4 | 0.126 0.151 0.150 0.119 0.126 0.126 0.075 0.126 5 | 0.239 0.000 0.121 0.121 0.124 0.121 0.121 0.152 6 | 0.173 0.173 0.000 0.000 0.000 0.481 0.173 0.000 7 | 0.000 0.000 0.134 0.134 0.000 0.464 0.134 0.134 8 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 9 | 0.091 0.000 0.000 0.728 0.091 0.091 0.000 0.000 10 | 0.140 0.140 0.300 0.140 0.000 0.140 0.000 0.140 11 | 0.130 0.130 0.000 0.480 0.130 0.000 0.130 0.000 12 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 13 | 0.106 0.106 0.106 0.106 0.366 0.106 0.000 0.106 14 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 15 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 16 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 -------------------------------------------------------------------------------- /conf/policies/pfsm/exploration_policy_evolved.txt: -------------------------------------------------------------------------------- 1 | 0.853 0.000 0.037 0.037 0.000 0.037 0.000 0.037 2 | 0.050 0.000 0.000 0.800 0.050 0.000 0.050 0.050 3 | 0.000 0.053 0.053 0.000 0.000 0.053 0.842 0.000 4 | 0.086 0.000 0.086 0.566 0.086 0.086 0.086 0.002 5 | 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 6 | 0.000 0.052 0.052 0.000 0.000 0.839 0.003 0.052 7 | 0.050 0.000 0.799 0.050 0.000 0.050 0.050 0.000 8 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 9 | 0.048 0.000 0.048 0.048 0.762 0.048 0.048 0.000 10 | 0.048 0.048 0.048 0.762 0.000 0.048 0.048 0.000 11 | 0.056 0.000 0.056 0.408 0.056 0.056 0.036 0.330 12 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 13 | 0.543 0.076 0.076 0.076 0.076 0.000 0.076 0.076 14 | 0.073 0.564 0.073 0.073 0.073 0.000 0.073 0.073 15 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 16 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 -------------------------------------------------------------------------------- /conf/policies/pfsm/exploration_policy_random.txt: -------------------------------------------------------------------------------- 1 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 2 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 3 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 4 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 5 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 6 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 7 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 8 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 9 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 10 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 11 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 12 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 13 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 14 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 15 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 16 | 0.125 0.125 0.125 0.125 0.125 0.125 0.125 0.125 17 | -------------------------------------------------------------------------------- /conf/policies/pfsm/aggregation_policy_benchmark.txt: -------------------------------------------------------------------------------- 1 | 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 2 | 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 3 | 0.000 -0.000 0.000 0.000 -0.000 0.000 0.000 1.000 4 | 0.357 0.643 0.000 0.000 0.000 0.000 0.000 0.000 5 | 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 6 | 0.382 0.130 0.012 0.168 0.000 0.083 0.006 0.219 7 | 0.000 0.000 0.000 0.000 0.000 -0.000 0.439 0.561 8 | 0.127 0.148 0.146 0.132 0.085 0.102 0.120 0.140 9 | 0.000 0.000 0.000 1.000 -0.000 0.000 0.000 0.000 10 | 0.328 0.193 0.263 0.071 0.145 -0.000 0.000 0.000 11 | 0.134 0.215 0.152 0.171 0.073 0.049 0.000 0.208 12 | 0.149 0.083 0.143 0.101 0.178 0.119 0.125 0.103 13 | 0.000 0.079 0.000 0.019 0.012 0.138 0.372 0.379 14 | 0.140 0.124 0.152 0.124 0.133 0.103 0.108 0.115 15 | 0.014 0.105 0.150 0.129 0.175 0.096 0.171 0.161 16 | 0.127 0.123 0.129 0.138 0.116 0.122 0.123 0.123 -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | { 5 | "name": "(gdb) Launch", 6 | "type": "cppdbg", 7 | "request": "launch", 8 | "program": "${workspaceRoot}/swarmulator", 9 | "args": ["1"], 10 | "stopAtEntry": false, 11 | "cwd": "${workspaceRoot}", 12 | "environment": [], 13 | "externalConsole": false, 14 | "MIMode": "gdb", 15 | "setupCommands": [ 16 | { 17 | "description": "Enable pretty-printing for gdb", 18 | "text": "-enable-pretty-printing", 19 | "ignoreFailures": true 20 | } 21 | ] 22 | } 23 | ] 24 | } 25 | -------------------------------------------------------------------------------- /scripts/python/README.md: -------------------------------------------------------------------------------- 1 | ### Install requirements 2 | 3 | Make sure you have python3 installed. 4 | To install the necessary sub-packages run: 5 | 6 | pip3 install -r requirements.txt 7 | 8 | # Main 9 | - `evolution_example.py`. This is an example file that evolves an aggregation controller for Swarmulator. Try it out and then use it as a baseline to make your own evolution! 10 | 11 | To run it use the command: 12 | python3 evolution_example.py aggregation particle 13 | 14 | You can replace aggregation and particle with the controller and agent of your choice! 15 | 16 | #### Classes 17 | - `evolution.py`. This is a wrapper around the DEAP python library for the main commands. 18 | You can edit it in order to change the way that the evolution is run. 19 | 20 | - `simulator.py`. This is a swarmulator API to command and interface with swarmulator from Python. 21 | 22 | 23 | -------------------------------------------------------------------------------- /sw/simulation/fifo.cpp: -------------------------------------------------------------------------------- 1 | #include "fifo.h" 2 | #include "settings.h" 3 | #include "main.h" 4 | #include // std::stringstream, std::stringbuf 5 | #include // write 6 | #include // fifo related 7 | #include // fifo related 8 | #include // fifo related 9 | 10 | fifo::fifo(std::string id) 11 | { 12 | fifo::open("/tmp/swarmulator_" + id); 13 | terminalinfo::debug_msg("Pipe set to " + id); 14 | }; 15 | 16 | void fifo::open(std::string file) 17 | { 18 | char const *bt_fifo_write = file.c_str(); 19 | if (access(bt_fifo_write, F_OK) == -1) { 20 | mkfifo(bt_fifo_write, 0666); 21 | } 22 | fifo_write_id = ::open(bt_fifo_write, O_RDWR | O_NONBLOCK); 23 | } 24 | 25 | bool fifo::send(float f) 26 | { 27 | uint16_t size = 8; 28 | char msg[size]; 29 | sprintf(msg, "%f", f); 30 | return write(fifo_write_id, (char *)msg, size * sizeof(char)); 31 | } 32 | -------------------------------------------------------------------------------- /sw/simulation/controllers/pattern_formation/README.md: -------------------------------------------------------------------------------- 1 | # aggregation 2 | 3 | ## Description 4 | This controller implements the pattern formation behavior as from the paper: 5 | 6 | **"Provable self-organizing pattern formation by a swarm of robots with limited knowledge."**. 7 | *Mario Coppola, Jian Guo, Eberhard Gill, Guido de Croon, 2019.* 8 | Mario Coppola, Jian Guo, Eberhard Gill, and Guido C. H. E. de Croon. Swarm Intelligence, 13(1):59--94, Mar 2019. 9 | 10 | The paper is available open-access at this link: 11 | https://link.springer.com/article/10.1007/s11721-019-00163-0 12 | 13 | The behavior is similar to the one, though, due to the evolutions of Swarmulator since then, it may differ slightly and may require some mild tuning to function in the same way. For the exact same behavior as in the paper, plese see the `SI_PatternFormation` branch of the repository. 14 | 15 | It is designed to work with AGENT=particle. -------------------------------------------------------------------------------- /sw/simulation/fifo.h: -------------------------------------------------------------------------------- 1 | #ifndef FIFO_H 2 | #define FIFO_H 3 | #include 4 | #include 5 | 6 | /** 7 | * Handles FIFO messaging for external interfacing. 8 | * 9 | */ 10 | class fifo 11 | { 12 | int fifo_write_id; 13 | 14 | public: 15 | /** 16 | * @brief Construct a new fifo object 17 | * 18 | */ 19 | fifo(std::string a); 20 | 21 | /** 22 | * @brief Destroy the fifo object 23 | * 24 | */ 25 | ~fifo() {}; 26 | 27 | /** 28 | * Create and open the FIFO pipe to communicate/interface with external programs, if needed. 29 | * By default, it is used to send the fitness. 30 | */ 31 | void open(std::string); 32 | 33 | /** 34 | * @brief Send out a FIFO message to interfact with external programs. 35 | * 36 | * @param fd FIFO file ID, initiated in the beginning of the thread 37 | * @return int 38 | */ 39 | bool send(float f); 40 | }; 41 | 42 | #endif /*FIFO_H*/ 43 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/blackboard.cpp: -------------------------------------------------------------------------------- 1 | #include "blackboard.h" 2 | 3 | // read data identified by string var from blackboard data manager 4 | double blackboard::get(const char *var, const int k /*= -1*/) 5 | { 6 | char name [255]; 7 | if (k >= 0) { 8 | sprintf(name, "%s%d", var, k); 9 | } else { 10 | strcpy(name, var); 11 | } 12 | 13 | if (blackboard::BB.find(name) == blackboard::BB.end()) { 14 | printf("get Requested element does not exist in the blackboard: %s %s\n", var, name); 15 | return 0.; 16 | } 17 | 18 | return blackboard::BB[name]; 19 | } 20 | 21 | // write data identified by string var to blackboard data manager 22 | void blackboard::set(const char *var, double data, const int k/* = -1*/) 23 | { 24 | char name [255]; 25 | if (k >= 0) { 26 | sprintf(name, "%s%d", var, k); 27 | } else { 28 | strcpy(name, var); 29 | } 30 | 31 | blackboard::BB[name] = data; 32 | } 33 | -------------------------------------------------------------------------------- /sw/simulation/controllers/leader_follower/README.md: -------------------------------------------------------------------------------- 1 | # leader_follower 2 | NOTE FOR USE: Modify particle_oriented.cpp to take vy_des rather than dpsirate! 3 | 4 | ## Description 5 | This controller implements the relative localization method and the state estimation from the paper: 6 | 7 | **"On-board range-based relative localization for micro air vehicles in indoor leader–follower flight"**. 8 | *Steven van der Helm, Mario Coppola, Kimberly N. McGuire, Guido C. H. E. de Croon, 2018. 9 | Autonomous Robots, March 2019, pp 1-27.* 10 | 11 | The paper is available open-access at this link: *https://link.springer.com/article/10.1007/s10514-019-09843-6* 12 | 13 | You can see it in action here: https://www.youtube.com/playlist?list=PL_KSX9GOn2P--aEr4JtFl7SV3LO5QZY4q 14 | 15 | The behavior is similar to the one, though note that it may differ slightly given its implementation within Swarmulator. 16 | 17 | Designed to work with AGENT=particle_oriented_xy -------------------------------------------------------------------------------- /sw/simulation/agents/wheeled/wheeled.h: -------------------------------------------------------------------------------- 1 | #ifndef WHEELED_H 2 | #define WHEELED_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "agent.h" 10 | 11 | class wheeled: public Agent 12 | { 13 | float r = 1; // Wheel radius 14 | float L = 10; // Distance between wheels 15 | public: 16 | /** 17 | * @brief Construct a new wheeled object 18 | * 19 | * @param i ID 20 | * @param state Initial state 21 | * @param tstep Simulation time step 22 | */ 23 | wheeled(int i, std::vector state, float tstep); 24 | 25 | /** 26 | * State update over one time step 27 | * 28 | * @param state current state 29 | * @return vector next state 30 | */ 31 | std::vector state_update(std::vector state); 32 | 33 | /** 34 | * Drawing to animate the wheeled agent 35 | * 36 | */ 37 | void animation(); 38 | }; 39 | 40 | #endif /*WHEELED_H*/ 41 | -------------------------------------------------------------------------------- /sw/simulation/controllers/aggregation/aggregation.h: -------------------------------------------------------------------------------- 1 | #ifndef AGGREGATION_H 2 | #define AGGREGATION_H 3 | 4 | #include "controller.h" 5 | #include "randomgenerator.h" 6 | #include "trigonometry.h" 7 | 8 | /** 9 | * This controller handles attraction and velocity in North and East separately 10 | */ 11 | class aggregation: public Controller 12 | { 13 | std::vector motion_p; // Probability of motion 14 | uint moving_timer; // Timer measuring how long a robot has been moving 15 | float vmean; 16 | float timelim; 17 | float v_x_ref, v_y_ref; 18 | uint st; 19 | 20 | public: 21 | 22 | /** 23 | * Construction. Controller_Aggregation is a child class of Controller. 24 | */ 25 | aggregation(); 26 | 27 | /** 28 | * Implementation of method to get the commanded velocity 29 | */ 30 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 31 | virtual void animation(const uint16_t ID); 32 | }; 33 | 34 | #endif /*AGGREGATION_H*/ 35 | -------------------------------------------------------------------------------- /sw/main.h: -------------------------------------------------------------------------------- 1 | #ifndef MAIN_H 2 | #define MAIN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "agent.h" 8 | #include "parameters.hxx" // Auto generated file at compile time 9 | #include "environment.h" 10 | #include "settings.h" 11 | 12 | extern uint nagents; // Number of agents in the swarm 13 | extern std::vector s; // Set up a vector of agents 14 | extern float simtime_seconds; // Adjusted simulation time (time according to simulation) 15 | extern std::shared_mutex mtx; // Mutex object 16 | extern std::shared_mutex mtx_env; // Mutex object 17 | extern bool program_running; // True if the program is (or should be) running. If false the program shuts down. 18 | extern std::unique_ptr param; // XML parameters from conf file 19 | extern float realtimefactor; // Real time factor of simulation 20 | extern Environment environment; // Vector holding the environment 21 | extern std::string identifier; // String identifier for FIFO pipe 22 | 23 | #endif /*MAIN_H*/ 24 | -------------------------------------------------------------------------------- /sw/simulation/controllers/pattern_formation/lattice.cpp: -------------------------------------------------------------------------------- 1 | #include "lattice.h" 2 | #include "agent.h" 3 | #include "main.h" 4 | #include "randomgenerator.h" 5 | #include "trigonometry.h" 6 | #include "draw.h" 7 | 8 | using namespace std; 9 | #define SENSOR_MAX_RANGE 1.8 10 | 11 | lattice::lattice() : lattice_basic() 12 | { 13 | beta_des.push_back(0.0); 14 | beta_des.push_back(atan(_ddes_y / _ddes_x)); 15 | beta_des.push_back(M_PI / 2.0); 16 | beta_des.push_back(M_PI / 2.0 + atan(_ddes_x / _ddes_y)); 17 | }; 18 | 19 | void lattice::get_velocity_command(const uint16_t ID, float &v_x, float &v_y) 20 | { 21 | v_x = 0; 22 | v_y = 0; 23 | 24 | vector state(8, 0); 25 | vector state_ID; 26 | // The ID is just used for simulation purposes 27 | t.assess_situation(ID, state, state_ID); 28 | vector closest = o.request_closest(ID); 29 | get_lattice_motion_all(ID, state_ID, closest, v_x, v_y); 30 | } 31 | 32 | void lattice::animation(const uint16_t ID) 33 | { 34 | draw d; 35 | d.circle_loop(SENSOR_MAX_RANGE); 36 | } -------------------------------------------------------------------------------- /sw/logger/txtwrite.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include // needed for std::setprecision 3 | 4 | #include "txtwrite.h" 5 | #include "omniscient_observer.h" 6 | #include "terminalinfo.h" 7 | #include "agent.h" 8 | #include "main.h" 9 | #include "fitness_functions.h" 10 | 11 | using namespace std; 12 | 13 | txtwrite::txtwrite() {} 14 | 15 | void txtwrite::setfilename(const string &s) 16 | { 17 | filename = s; // Set the name of the file 18 | } 19 | 20 | void txtwrite::txtwrite_state(ofstream &logfile) 21 | { 22 | std::stringstream t; // time 23 | t << simtime_seconds; // Write down time 24 | vector state_buff = s; // Get state 25 | float f = evaluate_fitness(); // Evaluate fitness 26 | for (uint16_t i = 0; i < s.size(); i++) { 27 | logfile << t.str() << " " // time 28 | << i + 1 << " "; // ID 29 | for (uint16_t j = 0; j < 2; j++) { // position state 0 and 1 30 | logfile << state_buff[i]->state.at(j) << " "; // log states 31 | } 32 | logfile << f; // fitness 33 | logfile << endl; // new line 34 | } 35 | } -------------------------------------------------------------------------------- /scripts/run_n_simulations.sh: -------------------------------------------------------------------------------- 1 | # Bash script to run a simulation multiple times and save all logs in a folder 2 | # This script expects swarmulator to have a kill-switch activated somewhere, 3 | # although manual quitting (pressing the 'q' key) is also fine. 4 | # $1 = number of trials 5 | # $2 = number of agents 6 | 7 | cd .. 8 | 9 | Make logs directory 10 | min=$(date +%Y-%m-%d-%T); 11 | d=logs/batchtest_$min 12 | mkdir $d 13 | 14 | for (( i = 1; i <= $1; i++ )); do 15 | 16 | # Bash text 17 | echo "Running trial $i out of $1 for a simulation with $2 agents" 18 | 19 | # Run code 20 | md=$(date +%Y-%m-%d-%T); 21 | ma=$(date +%Y-%m-%d-%T -d "+1 seconds") #backup time 22 | ./swarmulator $2 23 | sleep 1 # Give it some time to close 24 | 25 | # Move latest log to directory 26 | fn=$(ls -t logs| head -n1) 27 | if mv -f -- logs/log_$md.txt $d/log_$i.txt; then 28 | echo "Successfully moved file" 29 | else 30 | #if it doesn't work it's because it skipped a second 31 | echo "Trying again........!" 32 | mv -f -- logs/log_$ma.txt $d/log_$i.txt 33 | fi 34 | 35 | done 36 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/conditions/greater_than.h: -------------------------------------------------------------------------------- 1 | #ifndef GREATER_THAN_H 2 | #define GREATER_THAN_H 3 | 4 | #include "condition.h" 5 | 6 | namespace BT 7 | { 8 | /** 9 | * Class to check the great_than condition 10 | */ 11 | class greater_than : public condition 12 | { 13 | public: 14 | /** 15 | * @brief Constructor 1 for a new greater_than object 16 | */ 17 | greater_than(std::string vehicle_name, size_t param = MAX_SIZE) 18 | : condition(vehicle_name, "greater_than", param) 19 | { 20 | } 21 | /** 22 | * @brief Constructor 2 for a new greater_than object 23 | */ 24 | greater_than(std::string vehicle_name, size_t param, double value) 25 | : condition(vehicle_name, "greater_than", param, value) 26 | { 27 | } 28 | 29 | BT_Status update(blackboard *BLKB) 30 | { 31 | double data = BLKB->get(var.data()); 32 | if (data > limit) { // Check whether the data in the condition is greater than than the limit 33 | return BH_SUCCESS; 34 | } else { 35 | return BH_FAILURE; 36 | } 37 | } 38 | }; 39 | 40 | } 41 | 42 | #endif // GREATER_THAN_H -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/conditions/less_than.h: -------------------------------------------------------------------------------- 1 | #ifndef LESS_THAN_H 2 | #define LESS_THAN_H 3 | 4 | #include "condition.h" 5 | 6 | namespace BT 7 | { 8 | /** 9 | * Class for the less_than condition 10 | * 11 | */ 12 | class less_than : public condition 13 | { 14 | public: 15 | /** 16 | * Constructor 1 for a new less_than object 17 | */ 18 | less_than(std::string vehicle_name, size_t param = MAX_SIZE) 19 | : condition(vehicle_name, "less_than", param) 20 | { 21 | } 22 | /** 23 | * Constructor 2 for a new less_than object 24 | */ 25 | less_than(std::string vehicle_name, size_t param, double value) 26 | : condition(vehicle_name, "less_than", param, value) 27 | { 28 | } 29 | 30 | /** 31 | * @brief Implementation of how the condition is handled. 32 | * @param BLKB Running blackboard 33 | */ 34 | BT_Status update(blackboard *BLKB) 35 | { 36 | double data = BLKB->get(var.data()); 37 | if (data < limit) { // Check whether the data in the condition is less than than the limit 38 | return BH_SUCCESS; 39 | } else { 40 | return BH_FAILURE; 41 | } 42 | } 43 | }; 44 | 45 | } 46 | #endif // LESS_THAN_H -------------------------------------------------------------------------------- /sw/logger/txtwrite.h: -------------------------------------------------------------------------------- 1 | #ifndef TXTWRITE_H 2 | #define TXTWRITE_H 3 | 4 | #include // system, NULL, EXIT_FAILURE 5 | #include 6 | #include // std::stringstream, std::stringbuf 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include // std::stringstream, std::stringbuf 18 | #include "drawingparams.h" 19 | 20 | /** 21 | * Class to write data to a txt file, used for logging purposes 22 | */ 23 | class txtwrite 24 | { 25 | public: 26 | std::string filename; // Name of the log file 27 | 28 | /** 29 | * @brief Construct a new txtwrite object 30 | * 31 | */ 32 | txtwrite(); 33 | 34 | /** 35 | * Set the name of the txt file to be used 36 | * 37 | * @param s The name of the txt file 38 | */ 39 | void setfilename(const std::string &s); 40 | 41 | /** 42 | * Write the state of a robot to a txt file 43 | * 44 | * @param logfile The txt file ID (from ofstream) 45 | */ 46 | void txtwrite_state(std::ofstream &logfile); 47 | }; 48 | 49 | 50 | #endif /*WRITE_H*/ 51 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/btFile.h: -------------------------------------------------------------------------------- 1 | #ifndef BTFILE_H 2 | #define BTFILE_H 3 | 4 | #include "btCommon.h" 5 | 6 | namespace BT 7 | { 8 | 9 | /** 10 | * @brief Load a behavior tree file 11 | * 12 | * @param file The name of the file 13 | * @return composite* The composite tree structure 14 | */ 15 | BT::composite *loadFile(const char *file); 16 | 17 | /** 18 | * @brief 19 | * 20 | * @param file 21 | * @param task 22 | * @return true Success 23 | * @return false Failure 24 | */ 25 | bool saveFile(const char *file, composite *task); 26 | 27 | /** 28 | * @brief 29 | * 30 | * @param myfile 31 | * @param parent 32 | * @param tabs 33 | */ 34 | void writeComposite(std::ofstream &myfile, composite *parent, size_t tabs); 35 | 36 | /** 37 | * @brief 38 | * 39 | * @param Parent 40 | * @param line 41 | * @return node* 42 | */ 43 | node *newTask(node *Parent, std::string line); 44 | 45 | /** 46 | * @brief 47 | * 48 | * @param line 49 | * @return node* 50 | */ 51 | node *decodeLine(std::string line); 52 | 53 | /** 54 | * @brief 55 | * 56 | * @param task 57 | * @return std::string 58 | */ 59 | std::string codeLine(node *task); 60 | } 61 | 62 | #endif // BTFILE_H 63 | -------------------------------------------------------------------------------- /conf/parameters.xsd: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /sw/simulation/controllers/boid/boid.cpp: -------------------------------------------------------------------------------- 1 | #include "boid.h" 2 | #include "draw.h" 3 | #include "main.h" 4 | #include "auxiliary.h" 5 | 6 | #define K_NEAREST_BOID 3 7 | #define WALL_SENSOR 2.5 8 | 9 | using namespace std; 10 | boid::boid() : Controller() 11 | { 12 | } 13 | 14 | void boid::get_velocity_command(const uint16_t ID, float &v_x, float &v_y) 15 | { 16 | v_x = 0.1; 17 | v_y = 0; 18 | 19 | if (!wall_avoidance_turn(ID, v_x, v_y, WALL_SENSOR)) {// can see walls at 2.5 m away 20 | // Get the k nearest neighbors, here k = 5 21 | get_lattice_motion_k_nearest(ID, v_x, v_y, K_NEAREST_BOID); 22 | 23 | vector closest = o.request_closest(ID); 24 | float avg_psi = 0.; 25 | for (int i = 0; i < min(int(closest.size()), int(K_NEAREST_BOID)); i++) { 26 | avg_psi += s[closest[i]]->state[6] / float(min(int(closest.size()), int(K_NEAREST_BOID))); 27 | } 28 | v_y = avg_psi - s[ID]->state[6]; 29 | } 30 | } 31 | 32 | void boid::animation(const uint16_t ID) 33 | { 34 | vector r, b; 35 | o.relative_location(ID, r, b); 36 | draw d; 37 | for (int i = 0; i < min(int(r.size()), int(K_NEAREST_BOID)); i++) { 38 | float x, y; 39 | polar2cart(r[i], b[i], x, y); 40 | d.line(x, y, 0.5); 41 | } 42 | } -------------------------------------------------------------------------------- /conf/parameters.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 20 5 | 300 6 | 0 7 | 8 | square 9 | 11 | 12 | 800 13 | 800 14 | 0.2 15 | 0.3 16 | 0.4 17 | -40 18 | 19 | 25 20 | 21 | 2 22 | 23 | 24 | aggregation_clusters 25 | 26 | 27 | -------------------------------------------------------------------------------- /sw/animation/terminalinfo.cpp: -------------------------------------------------------------------------------- 1 | #include "terminalinfo.h" 2 | #include "main.h" 3 | 4 | void terminalinfo::debug_msg(std::string str) 5 | { 6 | #ifdef VERBOSE // Defined in the makefile! 7 | std::cout << "\e[01;36m[DEBUG]: \e[0m" << str << std::endl; 8 | #endif 9 | } 10 | 11 | void terminalinfo::debug_msg(std::string str, int ID) 12 | { 13 | #ifdef VERBOSE // Defined in the makefile! 14 | std::cout << "\e[01;36m[DEBUG]: \e[0m" << "Robot " << ID << ":\t" << str << std::endl; 15 | #endif 16 | } 17 | 18 | void terminalinfo::info_msg(std::string str) 19 | { 20 | #ifdef VERBOSE // Defined in the makefile! 21 | std::cout << "\e[01;34m[INFO]: \e[0m" << str << std::endl; 22 | #endif 23 | } 24 | 25 | void terminalinfo::info_msg(std::string str, int ID) 26 | { 27 | #ifdef VERBOSE // Defined in the makefile! 28 | std::cout << "\e[01;34m[INFO]: \e[0m" << "Robot " << ID << ":\t" << str << std::endl; 29 | #endif 30 | } 31 | 32 | void terminalinfo::warning_msg(std::string str) 33 | { 34 | #ifdef VERBOSE // Defined in the makefile! 35 | std::cout << "\e[01;33m[WARNING]: \e[0m" << str << std::endl; 36 | #endif 37 | } 38 | 39 | void terminalinfo::error_msg(std::string str) 40 | { 41 | std::cout << "\e[01;31m[ERROR]: \e[0m" << str << std::endl; 42 | program_running = false; 43 | } -------------------------------------------------------------------------------- /sw/simulation/controllers/pattern_formation/pattern_formation.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_PATTERN 2 | #define CONTROLLER_PATTERN 3 | 4 | #include "lattice_basic.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "terminalinfo.h" 13 | #include "template_calculator.h" 14 | 15 | /** 16 | * This controller makes a pattern from a lattice. 17 | * It is a child to lattice_basic, which handles the lattice control 18 | */ 19 | class pattern_formation : public lattice_basic 20 | { 21 | uint moving_timer; // Internal timer 22 | int selected_action; // Selected direction of motion within lattice 23 | 24 | public: 25 | /** 26 | * @brief Construct a new pattern formation object 27 | */ 28 | pattern_formation(); 29 | 30 | /** 31 | * @brief Destroy the pattern formation object 32 | */ 33 | ~pattern_formation() {}; 34 | 35 | /** 36 | * @brief Get the velocity command object 37 | * 38 | * @param ID 39 | * @param v_x 40 | * @param v_y 41 | */ 42 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 43 | 44 | /** 45 | * Draw the controller 46 | */ 47 | virtual void animation(const uint16_t ID); 48 | }; 49 | 50 | #endif /*CONTROLLER_PATTERN*/ -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/behavior_tree.h: -------------------------------------------------------------------------------- 1 | #ifndef BEHAVIOR_TREE_H 2 | #define BEHAVIOR_TREE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "controller.h" 8 | #include "terminalinfo.h" 9 | #include "randomgenerator.h" 10 | 11 | // Include behavior tree library 12 | #include "bt/btFile.h" 13 | using namespace BT; 14 | 15 | class behavior_tree: public Controller 16 | { 17 | public: 18 | float v_x_ref, v_y_ref, ang; 19 | uint moving_timer; // Timer measuring how long a robot has been moving 20 | composite *tree; // Tree structure. 21 | blackboard BLKB; // Blackboard structure that ticks the tree during runtime. 22 | 23 | /** Initialize 24 | * Initialize the behavior tree 25 | */ 26 | behavior_tree(); 27 | 28 | /** 29 | * Function for lattice to all robots 30 | * 31 | * @param closest vector of ID of all closest neighbors 32 | * @param v_x 33 | * @param v_y 34 | */ 35 | void lattice_all(const int &ID, const std::vector &closest, float &v_x, float &v_y); 36 | 37 | /** 38 | * Update the inputs and run the behavior tree command 39 | */ 40 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 41 | virtual void animation(const uint16_t ID); 42 | }; 43 | 44 | #endif /*BEHAVIOR_TREE_H*/ 45 | -------------------------------------------------------------------------------- /sw/simulation/agents/particle/particle.cpp: -------------------------------------------------------------------------------- 1 | #include "particle.h" 2 | #include "trigonometry.h" 3 | #include "draw.h" 4 | 5 | particle::particle(int i, std::vector s, float tstep) 6 | { 7 | state = s; 8 | ID = i; 9 | dt = tstep; 10 | orientation = 0.0; 11 | controller->set_saturation(1.0); 12 | } 13 | 14 | std::vector particle::state_update(std::vector state) 15 | { 16 | // NED frame 17 | // x+ towards North 18 | // y+ towards East 19 | 20 | float v_x = 0.0; 21 | float v_y = 0.0; 22 | controller->get_velocity_command(ID, v_x, v_y); 23 | controller->saturate(v_x); 24 | controller->saturate(v_y); 25 | moving = controller->moving; 26 | 27 | float vxr, vyr; 28 | rotate_xy(v_x, v_y, orientation, vxr, vyr); 29 | 30 | // Acceleration 31 | state.at(4) = 2 * (vxr - state[2]); // Acceleration x 32 | state.at(5) = 2 * (vyr - state[3]); // Acceleration y 33 | 34 | // Velocity 35 | state.at(2) += state[4] * dt; // Velocity x 36 | state.at(3) += state[5] * dt; // Velocity y 37 | 38 | // Position 39 | state.at(0) += state[2] * dt + 0.5 * state[4] * pow(dt, 2); // Position x 40 | state.at(1) += state[3] * dt + 0.5 * state[5] * pow(dt, 2); // Position y 41 | 42 | return state; 43 | }; 44 | 45 | void particle::animation() 46 | { 47 | draw d; 48 | d.circle(param->scale()); 49 | } 50 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/behavior_tree_wheeled.h: -------------------------------------------------------------------------------- 1 | #ifndef BEHAVIOR_TREE_WHEELED_H 2 | #define BEHAVIOR_TREE_WHEELED_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "controller.h" 8 | #include "terminalinfo.h" 9 | #include "randomgenerator.h" 10 | 11 | // Include behavior tree library 12 | #include "bt/btFile.h" 13 | using namespace BT; 14 | 15 | class behavior_tree_wheeled: public Controller 16 | { 17 | public: 18 | float v_x_ref, v_y_ref, ang; 19 | uint moving_timer; // Timer measuring how long a robot has been moving 20 | composite *tree; // Tree structure. 21 | blackboard BLKB; // Blackboard structure that ticks the tree during runtime. 22 | 23 | /** Initialize 24 | * Initialize the behavior tree 25 | */ 26 | behavior_tree_wheeled(); 27 | 28 | /** 29 | * Function for lattice to all robots 30 | * 31 | * @param closest vector of ID of all closest neighbors 32 | * @param v_x 33 | * @param v_y 34 | */ 35 | void lattice_all(const int &ID, const std::vector &closest, float &v_x, float &v_y); 36 | 37 | /** 38 | * Update the inputs and run the behavior tree command 39 | */ 40 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y); 41 | virtual void animation(const uint16_t ID); 42 | }; 43 | 44 | #endif /*BEHAVIOR_TREE_WHEELED_H*/ 45 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/blackboard.h: -------------------------------------------------------------------------------- 1 | #ifndef BLACKBOARD_H 2 | #define BLACKBOARD_H 3 | 4 | #include 5 | #include "string.h" 6 | #include 7 | 8 | /** 9 | * blackboard 10 | * Used to store all data for the Behaviour Tree to access 11 | * get() - returns the value in the blackboard with name "var" 12 | * for multiple entries with the same name k is used as an iterator 13 | * throws an error if element does not exist in list 14 | * set() - sets the value of the variable named "var" 15 | * if var is not in the list it is added 16 | * for multiple entries with the same name k is used as an iterator 17 | */ 18 | struct blackboard { 19 | public: 20 | /** 21 | * @brief Construct a new blackboard object 22 | * 23 | */ 24 | blackboard() {} 25 | 26 | /** 27 | * @brief Destroy the blackboard object 28 | * 29 | */ 30 | ~blackboard() {} 31 | 32 | /** 33 | * @brief 34 | * 35 | * @param var 36 | * @param k 37 | * @return double 38 | */ 39 | double get(const char *var, const int k = -1); 40 | 41 | /** 42 | * @brief 43 | * 44 | * @param var 45 | * @param data 46 | * @param k 47 | */ 48 | void set(const char *var, double data, const int k = -1); 49 | private: 50 | 51 | std::map< std::string, double > BB; 52 | }; 53 | 54 | #endif // BLACKBOARD_H 55 | -------------------------------------------------------------------------------- /scripts/python/tools/matrixOperations.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Collection of tools for "less standard" math operations 4 | @author: Mario Coppola, 2020 5 | """ 6 | import numpy as np 7 | 8 | def round_to_multiple(a, mult, floor=True): 9 | '''Rounds a number to its multiple''' 10 | if floor: 11 | a = np.floor(a / mult) 12 | else: 13 | a = np.round(a / mult) 14 | return a * mult 15 | 16 | def normalize_rows(mat,axis=1): 17 | '''Normalzies the rows of a matrix''' 18 | row_sums = np.sum(mat, axis=axis) 19 | if not np.isscalar(row_sums): 20 | mat = np.divide(mat,row_sums[:,np.newaxis], out=np.zeros_like(mat), where=row_sums[:,np.newaxis]!=0) 21 | else: 22 | mat = np.divide(mat,row_sums) 23 | return mat 24 | 25 | def pagerank(G, tol=1e-8): 26 | '''Iterative procedure to solve for the PageRank vector''' 27 | G = normalize_rows(G) 28 | n = G.shape[0] 29 | pr = 1 / n * np.ones((1, n)) # Initialize PageRank vector 30 | residual = 1 # Initialize residual 31 | 32 | while residual >= tol: 33 | pr_previous = pr 34 | pr = np.matmul(pr,G) # Pagerank formula 35 | residual = np.linalg.norm(np.subtract(pr,pr_previous)) 36 | # print(residual) 37 | 38 | return normalize_rows(np.asarray(pr)) 39 | 40 | def pretty_print(mat): 41 | '''Prints a matrix to the terminal but it looks a little better''' 42 | for x in mat: 43 | print(*x, sep=" ") 44 | -------------------------------------------------------------------------------- /sw/simulation/agents/wheeled/wheeled.cpp: -------------------------------------------------------------------------------- 1 | #include "wheeled.h" 2 | #include "draw.h" 3 | #include "trigonometry.h" 4 | 5 | using namespace std; 6 | 7 | wheeled::wheeled(int i, vector s, float tstep) 8 | { 9 | state = s; 10 | ID = i; 11 | dt = tstep; 12 | orientation = state[6]; 13 | } 14 | 15 | vector wheeled::state_update(vector state) 16 | { 17 | float leftwheelspeed, rightwheelspeed; 18 | 19 | controller->get_velocity_command(ID, leftwheelspeed, rightwheelspeed); 20 | controller->saturate(leftwheelspeed); 21 | controller->saturate(rightwheelspeed); 22 | moving = controller->moving; 23 | 24 | // Model 25 | float v_x = r / 2 * leftwheelspeed + r / 2 * rightwheelspeed; 26 | float v_y = 0.0; 27 | float psi_rate = - r / L * leftwheelspeed + r / L * rightwheelspeed; 28 | 29 | // Orientation 30 | state.at(7) = psi_rate; // Orientation rate 31 | state.at(6) += state.at(7); // Orientation 32 | orientation = state.at(6); 33 | 34 | // Velocity 35 | float vxr, vyr; 36 | rotate_xy(v_x, v_y, orientation, vxr, vyr); // Local frame to global frame 37 | state.at(2) = vxr; // Velocity x 38 | state.at(3) = vyr; // Velocity y 39 | 40 | // Position 41 | state.at(0) += state[2] * dt; // Position x 42 | state.at(1) += state[3] * dt; // Position y 43 | 44 | return state; 45 | } 46 | 47 | 48 | void wheeled::animation() 49 | { 50 | draw d; 51 | d.triangle(param->scale()); 52 | } 53 | -------------------------------------------------------------------------------- /scripts/python/tools/fileHandler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Collection of functions to handle files, load them, save them, ect 4 | @author: Mario Coppola, 2020 5 | """ 6 | import numpy as np 7 | import os, time 8 | import glob 9 | 10 | def load_matrix(file): 11 | '''Loads a matrix from a file''' 12 | try: 13 | matrix = np.loadtxt(open(file, "rb"), delimiter=", \t", skiprows=1) 14 | return matrix 15 | except: 16 | raise ValueError("Matrix " + file + " could not be loaded! Exiting.") 17 | 18 | def read_matrix(folder, name, file_format=".csv"): 19 | mat = load_matrix(folder + name + file_format) 20 | return mat 21 | 22 | def make_folder(folder): 23 | '''Generates a folder if it doesn not exist''' 24 | try: 25 | os.mkdir(folder) 26 | except: 27 | None # Directory exists 28 | folder = folder + "/sim_" + time.strftime("%Y_%m_%d_%T") 29 | os.mkdir(folder) 30 | return folder + "/" 31 | 32 | def save_data(filename,*args): 33 | np.savez(filename,*args) 34 | print("Data saved") 35 | 36 | def save_to_txt(mat,name): 37 | NEWLINE_SIZE_IN_BYTES = -1 # -2 on Windows? 38 | with open(name, 'wb') as fout: # Note 'wb' instead of 'w' 39 | np.savetxt(fout, mat, delimiter=" ", fmt='%.3f') 40 | fout.seek(NEWLINE_SIZE_IN_BYTES, 2) 41 | fout.truncate() 42 | 43 | def get_latest_file(path): 44 | list_of_files = glob.glob(path) # * means all if need specific format then *.csv 45 | latest_file = max(list_of_files, key=os.path.getctime) 46 | return latest_file 47 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/actions/wheelSpeed.h: -------------------------------------------------------------------------------- 1 | #ifndef WHEELSPEED_H 2 | #define WHEELSPEED_H 3 | 4 | #include "node.h" 5 | 6 | namespace BT 7 | { 8 | 9 | struct wheelSpeed : public node { 10 | 11 | /** 12 | * @brief Update the value of the output on the blackboard 13 | * 14 | * @param BLKB 15 | * @return BT_Status 16 | */ 17 | BT_Status update(blackboard *BLKB); 18 | 19 | public: 20 | 21 | /** 22 | * @brief Construct a new wheelSpeed object 23 | * 24 | * @param left Left wheel speed (will be bounded within constructor) 25 | * @param right Right wheel speed (will be bounded within constructor) 26 | */ 27 | wheelSpeed(double left, double right) 28 | : node("robot", "action", "wheelSpeed"), 29 | leftWheelSpeed(left), 30 | rightWheelSpeed(right) 31 | { 32 | vars.push_back(left); 33 | vars_upper_lim.push_back(0); // Lower limit 34 | vars_lower_lim.push_back(1.0); // Upper limit 35 | 36 | vars.push_back(right); 37 | vars_upper_lim.push_back(0); // Lower limit 38 | vars_lower_lim.push_back(1.0); // Upper limit 39 | } 40 | 41 | /** 42 | * @brief Construct a new random wheelSpeed object 43 | * wheel speed randomly generated on interval [-0.5,0.5] with increments of 0.1 44 | */ 45 | wheelSpeed() 46 | : wheelSpeed((rand() % 101) / 100. - 0, (rand() % 101) / 100. - 0) 47 | {} 48 | 49 | private: 50 | double leftWheelSpeed; 51 | double rightWheelSpeed; 52 | }; 53 | 54 | } 55 | 56 | #endif // WHEELSPEED_H -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/robots/robotActions.cpp: -------------------------------------------------------------------------------- 1 | #include "robotActions.h" 2 | 3 | #include "wheelSpeed.h" 4 | 5 | size_t KACTION = 1; // total number of actions 6 | 7 | namespace BT 8 | { 9 | 10 | BT_Status wheelSpeed::update(blackboard *BLKB) 11 | { 12 | BLKB->set("wheelSpeed0", leftWheelSpeed); 13 | BLKB->set("wheelSpeed1", rightWheelSpeed); 14 | return BH_SUCCESS; 15 | } 16 | 17 | // Get action, when inputs are known 18 | node *getAction(std::string action, std::vector inputs /*= NULL*/) 19 | { 20 | node *task; 21 | if (inputs.empty()) { 22 | if (action.compare("wheelSpeed") == 0) { 23 | task = (node *) new wheelSpeed; 24 | } else { 25 | perror("Something is really wrong in :BT::Composite* getAction(std::string action, std::vector* inputs)"); 26 | } 27 | } else { 28 | if (action.compare("wheelSpeed") == 0) { 29 | task = (node *) new wheelSpeed(inputs[0], inputs[1]); 30 | } else { 31 | perror("Something is really wrong in :BT::Composite* getAction(std::string action, std::vector* inputs)"); 32 | } 33 | } 34 | 35 | return task; 36 | } 37 | 38 | // Get action, when inputs are known 39 | // Add all actions to the if else if list below 40 | node *getAction(size_t func /*= (size_t) - 1*/) 41 | { 42 | node *task; 43 | switch (func) { 44 | case 0: 45 | task = (node *) new wheelSpeed; 46 | break; 47 | default: 48 | //std::cerr << "ERROR in getAction(unsigned int func): number of actions out of bounds"<``` in your controller. 22 | 23 | 4. Go nuts! 24 | 25 | ## Troubleshooting and known prior issues 26 | - I get a compile error due to ```google/protobuf/...```. This is due to caffe2, which you may not need (and if you get this error you likely don't). For the purposes of swarmulator, you can hack a quick fix by deleting the ```caffe2``` folder in ```libtorch/include```. Or you can install the necessary packages :). 27 | 28 | - `````` not found. If this happens, check that TORCH_LIB_HOME is correctly defined as an environment variable. (pro tip: add it to bashrc) 29 | 30 | - Runtime error: ```./swarmulator: error while loading shared libraries: libc10.so: cannot open shared object file: No such file or directory```. 31 | Check that TORCH_LIB_HOME/lib is in LD_LIBRARY_PATH! 32 | -------------------------------------------------------------------------------- /sw/simulation/sensors/template_calculator.h: -------------------------------------------------------------------------------- 1 | #ifndef TEMPLATE_CALCULATOR_H 2 | #define TEMPLATE_CALCULATOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "terminalinfo.h" 11 | #include "omniscient_observer.h" 12 | 13 | class Template_Calculator 14 | { 15 | // Map of state-space index to possible action space indexes. 16 | OmniscientObserver o; // The omniscient observer is used to simulate sensing the other agents. 17 | float range; 18 | std::vector blink; 19 | uint sp; 20 | public: 21 | /** 22 | * Constructor 23 | */ 24 | Template_Calculator(uint segments, float rangesensor); 25 | 26 | /** 27 | * Destructor 28 | */ 29 | ~Template_Calculator() {}; 30 | 31 | /** 32 | * State-action map storage 33 | */ 34 | std::map> state_action_matrix; 35 | 36 | /** 37 | * Function to read the state-action map from a txt file and store it in the object 38 | */ 39 | void set_state_action_matrix(std::string filename); 40 | 41 | /** 42 | * Function to select the desired relative bearing based on what is easiest 43 | */ 44 | float get_preferred_bearing(const std::vector &bdes, const float v_b); 45 | 46 | /** 47 | * Construct the local discrete state in binary form (q) from the continous neighborhood 48 | */ 49 | bool fill_template(std::vector &q, const float &b_i, const float &u, const float &dmax, const float &angle_err); 50 | 51 | /** 52 | * Define the current neighborhood 53 | */ 54 | void assess_situation(uint16_t ID, std::vector &q, std::vector &q_ID); 55 | }; 56 | 57 | #endif /*TEMPLATE_CALCULATOR_H*/ 58 | -------------------------------------------------------------------------------- /scripts/fix_code_style.sh: -------------------------------------------------------------------------------- 1 | # Bash script to fix the code style of a given cpp or h file according to preferred specifications 2 | 3 | if ! type "astyle" > /dev/null; then 4 | echo "Install astyle in order to use this script" 5 | exit 1 6 | fi 7 | 8 | if [ $# -eq 0 ]; then 9 | echo "Please provide one or multiple files as arguments" 10 | exit 1 11 | fi 12 | 13 | ASTYLE_VERSION=`astyle --version 2>&1| awk '{print $4}'` 14 | echo "Using astyle version $ASTYLE_VERSION" 15 | 16 | set -f 17 | 18 | if [[ $(bc <<< "$ASTYLE_VERSION >= 2.03") -eq 1 ]]; then 19 | astyle --style=kr \ 20 | --indent=spaces=2 \ 21 | --convert-tabs \ 22 | --indent-switches \ 23 | --indent-preprocessor \ 24 | --pad-oper \ 25 | --pad-header \ 26 | --unpad-paren \ 27 | --keep-one-line-blocks \ 28 | --keep-one-line-statements \ 29 | --align-pointer=name \ 30 | --suffix=none \ 31 | --lineend=linux \ 32 | --add-brackets \ 33 | --ignore-exclude-errors-x \ 34 | --max-code-length=120 \ 35 | --recursive \ 36 | --quiet \ 37 | $* 38 | else 39 | astyle --style=kr \ 40 | --indent=spaces=2 \ 41 | --convert-tabs \ 42 | --indent-switches \ 43 | --indent-preprocessor \ 44 | --pad-oper \ 45 | --pad-header \ 46 | --unpad-paren \ 47 | --keep-one-line-blocks \ 48 | --keep-one-line-statements \ 49 | --align-pointer=name \ 50 | --suffix=none \ 51 | --lineend=linux \ 52 | --add-brackets \ 53 | --recursive \ 54 | --quiet \ 55 | $* 56 | fi 57 | -------------------------------------------------------------------------------- /sw/logger/logger_thread.h: -------------------------------------------------------------------------------- 1 | #ifndef LOGGER_THREAD_H 2 | #define LOGGER_THREAD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "main.h" 13 | #include "txtwrite.h" 14 | 15 | bool logger_running = false; 16 | float simtime_seconds_old; // Used to log at a given frequency 17 | 18 | /** 19 | * Run the logger 20 | * 21 | * @param logfile The file ID (ofstream) 22 | * @param filename The name of the logfile 23 | */ 24 | void run_logger(std::ofstream &logfile, std::string filename) 25 | { 26 | static txtwrite writer; 27 | if (!logger_running) { 28 | terminalinfo::info_msg("Logger started."); 29 | writer.setfilename(filename); // Set the filename 30 | logger_running = true; // Set logger running to true 31 | } 32 | 33 | // Write the logfile 34 | if (!paused && simtime_seconds > simtime_seconds_old + 1. / param->logger_updatefreq()) { 35 | mtx.lock_shared(); 36 | writer.txtwrite_state(logfile); 37 | simtime_seconds_old = simtime_seconds; 38 | mtx.unlock_shared(); 39 | } 40 | if (!program_running) {std::terminate();} 41 | } 42 | 43 | /** 44 | * Logger thread that logs the simulation to a txt file 45 | */ 46 | void main_logger_thread() 47 | { 48 | // Log filename 49 | std::string filename; 50 | filename = "logs/log_" + identifier + ".txt"; 51 | 52 | // Open the logfile for writing 53 | std::ofstream logfile; 54 | logfile.open(filename.c_str()); 55 | 56 | simtime_seconds_old = 0; 57 | // Initiate the logger 58 | while (program_running) { 59 | run_logger(logfile, filename); 60 | }; 61 | } 62 | 63 | #endif /*LOGGER_THREAD_H*/ 64 | -------------------------------------------------------------------------------- /sw/animation/terminalinfo.h: -------------------------------------------------------------------------------- 1 | #ifndef TERMINALINFO_H 2 | #define TERMINALINFO_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** 10 | * terminalinfo is used to print info commands to the terminal. 11 | * These can come in the form of info messages or debug messages 12 | * The debug and info flags are activated (or deactivated) in the makefile 13 | */ 14 | class terminalinfo 15 | { 16 | public: 17 | /** 18 | * Constructor 19 | */ 20 | terminalinfo() {}; 21 | 22 | /** 23 | * Destructor 24 | */ 25 | ~terminalinfo() {}; 26 | 27 | /** 28 | * Print a debug message, only if debug messages are turned on 29 | * 30 | * @param str Debug string to print 31 | */ 32 | static void debug_msg(std::string str); 33 | 34 | /** 35 | * Print a debug message, only if debug messages are turned on 36 | * 37 | * @param str Debug string to print 38 | * @param ID Robot ID 39 | */ 40 | static void debug_msg(std::string str, int ID); 41 | 42 | /** 43 | * Print an info message, only if info messages are turned on 44 | * 45 | * @param str Info string to print 46 | */ 47 | static void info_msg(std::string str); 48 | 49 | /** 50 | * Print an info message, only if warnings are turned on 51 | * 52 | * @param str Info string to print 53 | */ 54 | static void info_msg(std::string str, int ID); 55 | 56 | /** 57 | * Print a warning message, only if warnings are turned on 58 | * 59 | * @param str Warning string to print 60 | */ 61 | static void warning_msg(std::string str); 62 | 63 | /** 64 | * Print an error message 65 | * 66 | * @param str Error string to print 67 | */ 68 | static void error_msg(std::string str); 69 | }; 70 | 71 | #endif /* TERMINALINFO_H */ -------------------------------------------------------------------------------- /sw/math/randomgenerator.cpp: -------------------------------------------------------------------------------- 1 | #include "randomgenerator.h" 2 | #include 3 | #include "stdint.h" 4 | #include "auxiliary.h" 5 | 6 | random_generator::random_generator() 7 | { 8 | std::random_device rd; 9 | generator.seed(rd()); 10 | }; 11 | 12 | float random_generator::uniform_float(float min, float max) 13 | { 14 | std::uniform_real_distribution<> dist(min, max); 15 | return dist(generator); 16 | }; 17 | 18 | int random_generator::uniform_int(int min, int max) 19 | { 20 | std::uniform_int_distribution<> dist(min, max); 21 | return dist(generator); 22 | }; 23 | 24 | float random_generator::gaussian_float(float mean, float stddev) 25 | { 26 | std::normal_distribution<> dist(mean, stddev); 27 | return dist(generator); 28 | }; 29 | 30 | bool random_generator::bernoulli(float p) 31 | { 32 | std::bernoulli_distribution dist(p); 33 | return dist(generator); 34 | } 35 | 36 | 37 | int random_generator::discrete_int(std::vector &d) 38 | { 39 | std::discrete_distribution dist(d.begin(), d.end()); 40 | return dist(generator); 41 | } 42 | 43 | std::vector random_generator::gaussian_float_vector(const int &length, const float &mean, const float &std) 44 | { 45 | // Generate the random vector 46 | std::vector v(length, 0); 47 | for (uint16_t i = 0; i < length; i++) { 48 | v[i] = gaussian_float(mean, std); 49 | } 50 | 51 | return v; 52 | } 53 | 54 | /** 55 | * @brief 56 | * 57 | * @param length 58 | * @param min 59 | * @param max 60 | * @return vector 61 | */ 62 | std::vector random_generator::uniform_float_vector(const int &length, const float &min, const float &max) 63 | { 64 | // Generate the random vector 65 | std::vector v(length, 0); 66 | for (uint16_t i = 0; i < length; i++) { 67 | v[i] = uniform_float(min, max); 68 | } 69 | 70 | return v; 71 | } 72 | -------------------------------------------------------------------------------- /sw/math/graph.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef GRAPH_H 3 | #define GRAPH_H 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | /** 10 | * @brief Defines a graph structure that can be used to assess the topology of the swarm 11 | * 12 | * Code in part extracted from Sanfoundry Global Education & Learning Series 13 | * http://www.sanfoundry.com/cpp-program-check-undirected-graph-connected-bfs/ 14 | */ 15 | class Graph 16 | { 17 | private: 18 | int V; 19 | std::list *adj; 20 | 21 | public: 22 | /** 23 | * Constructor instatiates a list of all connections in the graph. 24 | * 25 | * @param V Total number of vertices 26 | */ 27 | Graph(int V) 28 | { 29 | this->V = V; 30 | adj = new std::list[V]; 31 | } 32 | 33 | /** 34 | * Function to add an edge in the topology graph between node v and w 35 | * 36 | * @param v ID of node v 37 | * @param w ID of node w 38 | */ 39 | void addEdge(int v, int w); 40 | 41 | /** 42 | * Returns the transpose of the graph 43 | */ 44 | Graph getTranspose(); 45 | 46 | /** 47 | * Breadth-First Search (BFS) implementation. See how it is used in graph::isConnected() 48 | * 49 | * @param s first node to be visited (use s=0) 50 | * @param visited visited nodes during BFS 51 | */ 52 | void BFS(int s, bool visited[]); 53 | 54 | /** 55 | * Depth-First Search 56 | * 57 | * @param v 58 | * @param visited 59 | */ 60 | void DFS(int v, bool visited[]); 61 | 62 | /** 63 | * @brief Checks that the topology is connected. 64 | * 65 | * Check whether the undirected graph is connected using BFS 66 | * Returns true if connected, false if not connected 67 | */ 68 | bool isConnected(); 69 | 70 | /** 71 | * Counts the number of connected robot clusters 72 | * 73 | * @return uint 74 | */ 75 | uint connectedComponents(); 76 | }; 77 | 78 | #endif /* GRAPH_H */ -------------------------------------------------------------------------------- /sw/simulation/controllers/leader_follower/leader_follower.h: -------------------------------------------------------------------------------- 1 | #ifndef NDI_FOLLOWER_H 2 | #define NDI_FOLLOWER_H 3 | #include "controller.h" 4 | #include "ekf_state_estimator.h" 5 | 6 | using namespace std; 7 | 8 | #define NDI_PAST_VALS 500 // Store the last 200 values in order to compute the control 9 | 10 | #define COMMAND_LOCAL 1 11 | 12 | typedef struct ndihandler { 13 | // Default values unless specified in constructor 14 | float delay = 4; 15 | float tau_x = 3; 16 | float tau_y = 3; 17 | float wn_x = 0.9; 18 | float wn_y = 0.9; 19 | float eps_x = 0.28; 20 | float eps_y = 0.28; 21 | float Kp = -1.5; 22 | float Ki = 0; 23 | float Kd = -3; 24 | float xarr[NDI_PAST_VALS]; 25 | float yarr[NDI_PAST_VALS]; 26 | float u1arr[NDI_PAST_VALS]; 27 | float v1arr[NDI_PAST_VALS]; 28 | float u2arr[NDI_PAST_VALS]; 29 | float v2arr[NDI_PAST_VALS]; 30 | float r1arr[NDI_PAST_VALS]; 31 | float ax2arr[NDI_PAST_VALS]; 32 | float ay2arr[NDI_PAST_VALS]; 33 | float tarr[NDI_PAST_VALS]; 34 | float gamarr[NDI_PAST_VALS]; 35 | int data_start = 0; 36 | int data_end = 0; 37 | int data_entries = 0; 38 | float commands[2]; 39 | float commands_lim[2]; 40 | } ndihandler; 41 | 42 | class leader_follower : public Controller 43 | { 44 | // The omniscient observer is used to simulate sensing the other agents. 45 | OmniscientObserver o; 46 | ndihandler ndihandle; 47 | ekf_state_estimator filter; 48 | 49 | public: 50 | leader_follower(); 51 | ~leader_follower() {}; 52 | 53 | float accessCircularFloatArrElement(float arr[], int index); 54 | float computeNdiFloatIntegral(float ndiarr[], float curtime); 55 | void cleanNdiValues(float tcur); 56 | bool ndi_follow_leader(void); 57 | void bindNorm(float max_command); 58 | void uwb_follower_control_periodic(void); 59 | virtual void get_velocity_command(const uint16_t ID, float &x_des, float &vy_des); 60 | virtual void animation(const uint16_t ID); 61 | }; 62 | 63 | #endif /*NDI_FOLLOWER_H*/ 64 | -------------------------------------------------------------------------------- /sw/simulation/agents/particle_oriented/particle_oriented.cpp: -------------------------------------------------------------------------------- 1 | #include "particle_oriented.h" 2 | #include "trigonometry.h" 3 | #include "randomgenerator.h" 4 | #include "draw.h" 5 | using namespace std; 6 | 7 | particle_oriented::particle_oriented(int i, vector s, float tstep) 8 | { 9 | state = s; 10 | ID = i; 11 | dt = tstep; 12 | orientation = state[6]; 13 | controller->set_saturation(0.5); 14 | manual = false; 15 | } 16 | 17 | vector particle_oriented::state_update(vector state) 18 | { 19 | // NED frame 20 | // x+ towards North 21 | // y+ towards East 22 | float vx_des, vy_des = 0.; 23 | float vx_global, vy_global, dpsirate; 24 | if (!manual) { 25 | controller->get_velocity_command(ID, vx_des, dpsirate); // Command comes out in the local frame 26 | } else { 27 | vx_des = manualx; 28 | vy_des = manualy; 29 | dpsirate = manualpsi_delta; 30 | } 31 | controller->saturate(vx_des); 32 | controller->saturate(vy_des); 33 | #if COMMAND_LOCAL 34 | rotate_xy(vx_des, vy_des, state[6], vx_global, vy_global); 35 | #else 36 | vx_global = vx_des; 37 | vy_global = vy_des; 38 | #endif 39 | state.at(7) = dpsirate; 40 | state.at(6) += state[7] * dt; 41 | orientation = wrapToPi_f(state[6]); 42 | 43 | // Acceleration control 44 | float ka = 2; 45 | state.at(4) = ka * (vx_global - state[2]); // Acceleration global frame 46 | state.at(5) = ka * (vy_global - state[3]); // Acceleration global frame 47 | moving = controller->moving; 48 | 49 | // Velocity 50 | state.at(2) += state[4] * dt; // Velocity x global frame 51 | state.at(3) += state[5] * dt; // Velocity y global frame 52 | 53 | // Position 54 | state.at(0) += state[2] * dt + 0.5 * state[4] * pow(dt, 2); // Position x global frame 55 | state.at(1) += state[3] * dt + 0.5 * state[5] * pow(dt, 2); // Position y global frame 56 | 57 | return state; 58 | }; 59 | 60 | void particle_oriented::animation() 61 | { 62 | draw d; 63 | 64 | d.triangle(param->scale()); 65 | } 66 | -------------------------------------------------------------------------------- /sw/simulation/agents/particle_oriented_xy/particle_oriented_xy.cpp: -------------------------------------------------------------------------------- 1 | #include "particle_oriented_xy.h" 2 | #include "trigonometry.h" 3 | #include "randomgenerator.h" 4 | #include "draw.h" 5 | using namespace std; 6 | 7 | particle_oriented_xy::particle_oriented_xy(int i, vector s, float tstep) 8 | { 9 | state = s; 10 | ID = i; 11 | dt = tstep; 12 | orientation = state[6]; 13 | controller->set_saturation(0.5); 14 | manual = false; 15 | } 16 | 17 | vector particle_oriented_xy::state_update(vector state) 18 | { 19 | // NED frame 20 | // x+ towards North 21 | // y+ towards East 22 | float vx_des, vy_des = 0.; 23 | float vx_global, vy_global, dpsirate = 0.; 24 | if (!manual) { 25 | controller->get_velocity_command(ID, vx_des, vy_des); // Command comes out in the local frame 26 | } else { 27 | vx_des = manualx; 28 | vy_des = manualy; 29 | dpsirate = manualpsi_delta; 30 | } 31 | controller->saturate(vx_des); 32 | controller->saturate(vy_des); 33 | #if COMMAND_LOCAL 34 | rotate_xy(vx_des, vy_des, state[6], vx_global, vy_global); 35 | #else 36 | vx_global = vx_des; 37 | vy_global = vy_des; 38 | #endif 39 | state.at(7) = dpsirate; 40 | state.at(6) += state[7] * dt; 41 | orientation = wrapToPi_f(state[6]); 42 | 43 | // Acceleration control 44 | float ka = 2; 45 | state.at(4) = ka * (vx_global - state[2]); // Acceleration global frame 46 | state.at(5) = ka * (vy_global - state[3]); // Acceleration global frame 47 | moving = controller->moving; 48 | 49 | // Velocity 50 | state.at(2) += state[4] * dt; // Velocity x global frame 51 | state.at(3) += state[5] * dt; // Velocity y global frame 52 | 53 | // Position 54 | state.at(0) += state[2] * dt + 0.5 * state[4] * pow(dt, 2); // Position x global frame 55 | state.at(1) += state[3] * dt + 0.5 * state[5] * pow(dt, 2); // Position y global frame 56 | 57 | return state; 58 | }; 59 | 60 | void particle_oriented_xy::animation() 61 | { 62 | draw d; 63 | 64 | d.triangle(param->scale()); 65 | } 66 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/behavior_tree_wheeled.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_tree_wheeled.h" 2 | #include "draw.h" 3 | #include "terminalinfo.h" 4 | #include "auxiliary.h" 5 | #include 6 | using namespace std; 7 | 8 | #define KNEAREST 6 9 | #define WALL_SENSOR 2.5 10 | 11 | // Defining the binary function 12 | bool comp(int a, int b) 13 | { 14 | return (a < b); 15 | } 16 | 17 | behavior_tree_wheeled::behavior_tree_wheeled() : Controller() 18 | { 19 | // Load the behavior tree 20 | tree = loadFile(param->policy().c_str()); 21 | 22 | // Initialize input sensors 23 | string name; 24 | for (size_t i = 0; i < KNEAREST; i++) { 25 | name = "sensor" + to_string(i); 26 | BLKB.set(name.c_str(), 0.); 27 | } 28 | 29 | // Initialize outputs 30 | BLKB.set("wheelSpeed0", 0.); // Output 0 31 | BLKB.set("wheelSpeed1", 0.); // Output 1 32 | } 33 | 34 | void behavior_tree_wheeled::get_velocity_command(const uint16_t ID, float &v_x, float &v_y) 35 | { 36 | v_x = 0; 37 | v_y = 0; 38 | 39 | vector r, b; 40 | o.relative_location_inrange(ID, WALL_SENSOR, r, b); 41 | 42 | // float vmean = 1.0; 43 | 44 | /**** Step 1 of 3: Set current state according to sensors ****/ 45 | if (r.size() > 0) { 46 | string name; 47 | for (size_t i = 0; i < std::min((uint)r.size(), (uint)KNEAREST); i++) { 48 | name = "sensor" + to_string(i); 49 | BLKB.set(name.c_str(), r[i]); 50 | } 51 | } 52 | 53 | /**** Step 2 of 3: Tick the tree based on the current state ****/ 54 | tree->tick(&BLKB); 55 | 56 | /**** Step 3 of 3: Set outputs (do this once, else keep!) ****/ 57 | float wheelSpeedleft = BLKB.get("wheelSpeed0"); 58 | float wheelSpeedright = BLKB.get("wheelSpeed1"); 59 | 60 | v_x += wheelSpeedleft; 61 | v_y += wheelSpeedright; 62 | 63 | string d = to_string(v_x) + ", " + to_string(v_y); 64 | terminalinfo::debug_msg(d, ID); 65 | } 66 | 67 | void behavior_tree_wheeled::animation(const uint16_t ID) 68 | { 69 | draw d; 70 | d.circle_loop(WALL_SENSOR); 71 | } -------------------------------------------------------------------------------- /scripts/make_new_controller.sh: -------------------------------------------------------------------------------- 1 | # Bash script to create the header file (*.h) and source (*.cpp) of a new controller 2 | # $1: Name of the controller 3 | 4 | if [ "$1" = "" ]; then 5 | echo "Please specify the name of the controller.\n"; 6 | exit 1; 7 | fi 8 | 9 | mkdir ../sw/simulation/controllers/$1 10 | 11 | folder=../sw/simulation/controllers/$1; 12 | 13 | if [ -f $folder/$1".h" ] || [ -f $folder/$1".cpp" ]; then 14 | echo "An agent with this name already exists. Do you want to over write it?" 15 | echo "[write yes and press Enter to continue]" 16 | read option; 17 | if [ "$option" != "yes" ]; then 18 | exit 0; 19 | fi 20 | fi 21 | 22 | str=$1 23 | c=`echo $str | awk '{print toupper($0)}'` 24 | 25 | awk -vn=$1 -vN=$c -vp=$folder 'BEGIN{ 26 | hN = p"/"n".h"; 27 | print "Creating header: "n".h ..."; 28 | print "#ifndef "N"_H" > hN; 29 | print "#define "N"_H\n" > hN; 30 | print "#include " > hN; 31 | print "#include " > hN; 32 | print "#include \"controller.h\"\n" > hN; 33 | print "class "n": public Controller" > hN; 34 | print "{" > hN; 35 | print "public:" > hN; 36 | print " "n"():Controller(){};" > hN; 37 | print " virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y);" > hN; 38 | print " virtual void animation(const uint16_t ID);" > hN; 39 | print "};\n" > hN; 40 | print "#endif /*"N"_H*/" >> hN; 41 | 42 | cN = p"/"n".cpp"; 43 | print "Creating file implementation: "n".cpp ..."; 44 | print "#include \""n".h\"" > cN; 45 | print "#include \"draw.h\"\n" > cN; 46 | print "void "n"::get_velocity_command(const uint16_t ID, float &v_x, float &v_y)\n{" > cN; 47 | print " /*** Put your controller here ***/\n}" >> cN; 48 | print "void "n"::animation(const uint16_t ID)\n{" > cN; 49 | print " /*** Put the animation of the controller/sensors here ***/\n}" >> cN; 50 | 51 | cN = p"/README.md"; 52 | print "Creating README file"; 53 | print "#"n"" > cN; 54 | print "Please include a description of the controller here, both for yourself and for future users!" > cN; 55 | }' 56 | 57 | exit 0; 58 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/node.h: -------------------------------------------------------------------------------- 1 | #ifndef BEHAVIOUR_H 2 | #define BEHAVIOUR_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "blackboard.h" 9 | 10 | namespace BT 11 | { 12 | 13 | /** 14 | * @brief Task status types 15 | * Important that order is maintained with more important states as higher values 16 | * 17 | */ 18 | enum BT_Status { 19 | BH_INVALID, 20 | BH_SUCCESS, 21 | BH_RUNNING, 22 | BH_SUSPENDED, 23 | BH_FAILURE 24 | }; 25 | 26 | /** 27 | * Base class for actions, conditions and composites 28 | */ 29 | class node 30 | { 31 | public: 32 | /** 33 | * @brief Construct a new node object 34 | * 35 | * @param Name 36 | * @param Type 37 | * @param Function 38 | */ 39 | node(std::string Name, std::string Type, std::string Function) 40 | : m_eStatus(BH_INVALID), 41 | name(Name), 42 | type(Type), 43 | function(Function), 44 | tick_counter(0) 45 | {} 46 | 47 | /** 48 | * @brief Destroy the node object 49 | * 50 | */ 51 | virtual ~node() 52 | { 53 | } 54 | 55 | // General 56 | 57 | /** 58 | * @brief Tick the current behavior tree 59 | * 60 | * @param BLKB Blackboard object keeping track of the current state 61 | * @return BT_Status 62 | */ 63 | BT_Status tick(blackboard *BLKB); 64 | 65 | /** 66 | * @brief Update the status of the behavior tree 67 | * 68 | * @param BLKB Blackboard object keeping track of the current state 69 | * @return BT_Status 70 | */ 71 | virtual BT_Status update(blackboard *BLKB) = 0; 72 | 73 | /** 74 | * @brief 75 | * 76 | */ 77 | virtual void onInitialise() {} 78 | 79 | /** 80 | * @brief 81 | * 82 | */ 83 | virtual void onTerminate(BT_Status) {} 84 | 85 | BT_Status m_eStatus; 86 | std::string name; 87 | std::string type; 88 | std::string function; 89 | size_t tick_counter; 90 | std::vector vars; // Node internal variables 91 | std::vector vars_upper_lim; 92 | std::vector vars_lower_lim; 93 | }; 94 | 95 | } 96 | #endif // BEHAVIOUR_H 97 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/README.md: -------------------------------------------------------------------------------- 1 | # behavior_tree 2 | 3 | ## Description 4 | This controller reads a behavior tree and runs the behavior accordingly. 5 | It used the behavior tree library in the sub-folder `bt`, which was originally written by Kirk Scheper, and has now been edited for inclusion within Swarmulator. 6 | 7 | ## Usage 8 | Sample behavior trees are defined in: `conf/behavior_trees/` 9 | You can use use your own behavior tree by changing the behavior tree file in use, which is defined in the controller's constructor. 10 | 11 | Note that it will only work if you follow the correct xml formatting for the behavior tree. 12 | 13 | ## Behavior tree xml formatting 14 | A sample behavior tree can look as following: 15 | ``` 16 | compositesequencesequence 17 | conditiongreater_than0,0.51robot 18 | actionwheelSpeed0.1066,0.1375robot 19 | ``` 20 | We have 4 types of nodes that we can use: 21 | 22 | - **Action** 23 | An action node is defined by, for example: 24 | `actionwheelSpeed0.1066,0.1375robot` 25 | The action sets the wheelSpeed in the behavior tree. It can be accessed in the behavior_tree.cpp through 26 | `BLKB.get("wheelSpeed0");` 27 | The command reads the 0th parameter of wheelSpeed (which in the example above = 0.1066) 28 | 29 | - **Condition** 30 | A condition node is defined by, for example: 31 | `conditiongreater_than0,0.51robot` 32 | A condition depends on the input to the behavior tree, which we set through 33 | `BLKB.set("sensor0",0.5);` 34 | This sets the sensor #0 to 0.5. 35 | In the example above, the condition will fail since 0.5<0.51 36 | 37 | - **Selector** 38 | A selector node (usually visualized as `?`) is declared as, for example: 39 | `compositeselectorselector` 40 | - **Sequence** 41 | A composite node (usually visualized as `!`) is defined as, for example: 42 | `compositesequencesequence` 43 | 44 | ## Troubleshoot tips 45 | - Make sure that there are no empty spaces at the end of the behavior tree xml file! -------------------------------------------------------------------------------- /sw/simulation/controllers/aggregation/aggregation.cpp: -------------------------------------------------------------------------------- 1 | #include "aggregation.h" 2 | #include "agent.h" 3 | #include "main.h" 4 | #include "randomgenerator.h" 5 | #include "auxiliary.h" 6 | #include "draw.h" 7 | 8 | #define SENSOR_MAX_RANGE 1.8 9 | 10 | using namespace std; 11 | aggregation::aggregation() : Controller() 12 | { 13 | // Initial values 14 | moving = false; 15 | moving_timer = rg.uniform_int(0, timelim); 16 | v_x_ref = rg.gaussian_float(0.0, 1.0); 17 | v_y_ref = rg.gaussian_float(0.0, 1.0); 18 | 19 | set_max_sensor_range(SENSOR_MAX_RANGE); 20 | 21 | // Control values 22 | timelim = 2.0 * param->simulation_updatefreq(); 23 | vmean = 0.5; 24 | 25 | // Policy 26 | if (!strcmp(param->policy().c_str(), "")) { motion_p.assign(7, 0.); motion_p[0] = 1.; } 27 | else { motion_p = read_array(param->policy()); } 28 | } 29 | 30 | void aggregation::get_velocity_command(const uint16_t ID, float &v_x, float &v_y) 31 | { 32 | v_x = 0; 33 | v_y = 0; 34 | 35 | get_lattice_motion_range(ID, v_x, v_y, SENSOR_MAX_RANGE); // Repulsion from neighbors 36 | 37 | // Sense neighbors 38 | vector r, b; 39 | o.relative_location_inrange(ID, SENSOR_MAX_RANGE, r, b); 40 | 41 | if (st != r.size() || moving_timer == 1) { // state change 42 | // state, action 43 | st = std::min(r.size(), motion_p.size()); 44 | if (rg.bernoulli(1.0 - motion_p[st])) { 45 | v_x_ref = 0.0; 46 | v_y_ref = 0.0; 47 | moving = false; 48 | } else { // Else explore randomly, change heading 49 | float ang = rg.uniform_float(0.0, 2 * M_PI); 50 | if (moving) { 51 | float ext = rg.gaussian_float(0.0, 0.5); 52 | float temp; 53 | cart2polar(v_x_ref, v_y_ref, temp, ang); 54 | ang += ext; 55 | } 56 | wrapTo2Pi(ang); 57 | polar2cart(vmean, ang, v_x_ref, v_y_ref); 58 | moving = true; 59 | } 60 | } 61 | increase_counter_to_value(moving_timer, timelim, 1); 62 | wall_avoidance_bounce(ID, v_x_ref, v_y_ref, SENSOR_MAX_RANGE); 63 | 64 | // Final output 65 | v_x += v_x_ref; 66 | v_y += v_y_ref; 67 | } 68 | 69 | void aggregation::animation(const uint16_t ID) 70 | { 71 | draw d; 72 | d.circle_loop(SENSOR_MAX_RANGE); 73 | } -------------------------------------------------------------------------------- /sw/simulation/controllers/leader_follower/ekf_state_estimator.cpp: -------------------------------------------------------------------------------- 1 | #include "ekf_state_estimator.h" 2 | #include "main.h" 3 | #include 4 | #include 5 | #include 6 | #include "trigonometry.h" 7 | 8 | // Initilizer 9 | ekf_state_estimator::ekf_state_estimator() 10 | { 11 | initialized = false; 12 | }; 13 | 14 | void ekf_state_estimator::init_ekf_filter() 15 | { 16 | float pxf, pyf; 17 | polar2cart(o.request_distance(ID, ID_tracked), o.request_bearing(ID, ID_tracked), pxf, pyf); 18 | discrete_ekf_no_north_new(&ekf_rl); 19 | ekf_rl.X[0] = pxf; 20 | ekf_rl.X[1] = pyf; 21 | ekf_rl.X[8] = wrapToPi_f(s[ID_tracked]->get_state(6) - s[ID]->get_state(6)); 22 | initialized = true; 23 | simtime_seconds_store = simtime_seconds; 24 | } 25 | 26 | void ekf_state_estimator::run_ekf_filter() 27 | { 28 | // All in local frame of follower!!!! Values for position, velocity, acceleration 29 | float vxf, vyf, vx0f, vy0f, axf, ayf, ax0, ay0; 30 | // Global to local, rotate the opposite of local to global, hence the negative 31 | rotate_xy(s[ID]->get_state(2), s[ID]->get_state(3), -s[ID]->get_state(6), vxf, vyf); 32 | rotate_xy(s[ID]->get_state(4), s[ID]->get_state(5), -s[ID]->get_state(6), axf, ayf); 33 | rotate_xy(s[ID_tracked]->get_state(2), s[ID_tracked]->get_state(3), -s[ID_tracked]->get_state(6), vx0f, vy0f); 34 | rotate_xy(s[ID_tracked]->get_state(4), s[ID_tracked]->get_state(5), -s[ID_tracked]->get_state(6), ax0, ay0); 35 | ekf_rl.dt = simtime_seconds - simtime_seconds_store; 36 | simtime_seconds_store = simtime_seconds; 37 | float U[EKF_L] = {axf, ayf, ax0, ay0, s[ID]->get_state(7), s[ID_tracked]->get_state(7)}; 38 | float Z[EKF_M] = {o.request_distance(ID, ID_tracked), 0.0, 0.0, vxf, vyf, vx0f, vy0f}; 39 | discrete_ekf_no_north_predict(&ekf_rl, U); 40 | discrete_ekf_no_north_update(&ekf_rl, Z); 41 | ekf_rl.X[8] = wrapToPi_f(ekf_rl.X[8]); 42 | } 43 | 44 | void ekf_state_estimator::run(uint16_t ID_in, uint16_t ID_tracked_in) 45 | { 46 | if (!initialized) { 47 | ID = ID_in; 48 | ID_tracked = ID_tracked_in; 49 | init_ekf_filter(); 50 | printf("Launched EKF instance for %d to %d\n", ID, ID_tracked); 51 | } else { 52 | run_ekf_filter(); 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /sw/simulation/agent.h: -------------------------------------------------------------------------------- 1 | #ifndef AGENT_H 2 | #define AGENT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "settings.h" 10 | #include "terminalinfo.h" 11 | #include CONTROLLER_INCLUDE 12 | 13 | /** 14 | * Parent class defining an agent. The dynamic implementation is handled in children classes. 15 | */ 16 | class Agent 17 | { 18 | public: 19 | /** 20 | * Constructor 21 | */ 22 | Agent(); 23 | 24 | /** 25 | * Destructor 26 | */ 27 | virtual ~Agent() {}; 28 | 29 | uint16_t ID; // ID of agent 30 | bool activated; // Set true if the agent is active 31 | float dt; 32 | std::vector state; // State vector 33 | float orientation; // Orientation 34 | bool moving; 35 | float manualx, manualy; 36 | bool manual; 37 | float manualpsi_delta; 38 | random_generator rg; 39 | Controller *controller = new CONTROLLER; 40 | 41 | /** 42 | * Returns the position of the agent along a certain dimension (i.e., North(0) and East(1)) 43 | * This is used by the OmniscientObserver class in order to simulate the presence of sensors. 44 | * @param dim dimension x or y 45 | * @return position along x or y 46 | */ 47 | float get_position(uint16_t dim); 48 | 49 | /** 50 | * Get the orientation object 51 | * 52 | * @return float 53 | */ 54 | float get_orientation(); 55 | 56 | /** 57 | * Get the state along a given dimension 58 | * 59 | * @param dim dimension 60 | * @return float 61 | */ 62 | float get_state(const uint16_t dim); 63 | 64 | /** 65 | * The agent class is only the parent class of a child class that specifies the dynamics and control 66 | * of a given agent (robot/animal/whatever). The state_update function is handled by the child class. 67 | * @param s current state vector 68 | * @return next state vector 69 | */ 70 | virtual std::vector state_update(std::vector) = 0; 71 | 72 | /** 73 | * The agent class is only the parent class of a child class that specifies the dynamics and control 74 | * of a given agent (robot/animal/whatever). The way that the agent looks is specified in the child class. 75 | */ 76 | virtual void animation() = 0; 77 | }; 78 | 79 | #endif /*AGENT_H*/ 80 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/condition.h: -------------------------------------------------------------------------------- 1 | #ifndef CONDITIONS_H 2 | #define CONDITIONS_H 3 | 4 | #include 5 | #include 6 | #include "node.h" 7 | #include "settings.h" 8 | 9 | #define MAX_SIZE (size_t) - 1 10 | #define KCOND 6 // Total number of sensors 11 | #define NUMBER_OF_VARS 6 // Total number of variables = sensors! 12 | 13 | 14 | /** 15 | * @brief Behavior tree namespace 16 | * 17 | * Holds the behavior tree structure. 18 | */ 19 | namespace BT 20 | { 21 | 22 | std::tuple input(int i); 23 | 24 | node *getCondition(std::string condition, size_t var = MAX_SIZE); 25 | node *getCondition(std::string condition, std::vector inputs); 26 | node *getCondition(size_t func = rand() % KCOND, size_t var = MAX_SIZE); 27 | 28 | class condition : public node 29 | { 30 | public: 31 | condition(std::string vehicle_name, std::string func_name, size_t param, double value) 32 | : node(vehicle_name, "condition", func_name), 33 | k_var(param), 34 | limit(value) 35 | { 36 | std::tuple set = input(k_var); 37 | var = std::get<0>(set); 38 | 39 | vars.push_back(double(k_var)); 40 | vars_lower_lim.push_back(double(k_var)); 41 | vars_upper_lim.push_back(double(k_var)); 42 | 43 | vars.push_back(limit); 44 | vars_lower_lim.push_back(std::get<2>(set)); 45 | vars_upper_lim.push_back(std::get<3>(set)); 46 | } 47 | condition(std::string vehicle_name, std::string func_name, size_t param = MAX_SIZE) 48 | : node(vehicle_name, "condition", func_name) 49 | { 50 | if (param >= NUMBER_OF_VARS) { 51 | k_var = rand() % NUMBER_OF_VARS; 52 | } else { 53 | k_var = param; 54 | } 55 | 56 | std::tuple set = input(k_var); 57 | 58 | var = std::get<0>(set); 59 | limit = std::get<1>(set); 60 | 61 | vars.push_back(double(k_var)); 62 | vars_lower_lim.push_back(double(k_var)); 63 | vars_upper_lim.push_back(double(k_var)); 64 | 65 | vars.push_back(limit); 66 | vars_lower_lim.push_back(std::get<2>(set)); 67 | vars_upper_lim.push_back(std::get<3>(set)); 68 | } 69 | 70 | size_t k_var; // Parameter number 71 | std::string var; // Parameter name 72 | double limit; // threshold 73 | }; 74 | 75 | } 76 | #endif // CONDITIONS_H 77 | -------------------------------------------------------------------------------- /scripts/make_new_agent.sh: -------------------------------------------------------------------------------- 1 | # Bash script to create the header file (*.h) and source (*.cpp) of a new agent 2 | # $1: Name of the agent 3 | 4 | if [ "$1" = "" ]; then 5 | echo "Please specify the name of the agent.\n"; 6 | exit 1; 7 | fi 8 | 9 | mkdir ../sw/simulation/agents/$1 10 | 11 | folder=../sw/simulation/agents/$1; 12 | 13 | if [ -f $folder/$1".h" ] || [ -f $folder/$1".cpp" ]; then 14 | echo "An agent with this name already exists. Do you want to over write it?" 15 | echo "[write yes and press Enter to continue]" 16 | read option; 17 | if [ "$option" != "yes" ]; then 18 | exit 0; 19 | fi 20 | fi 21 | 22 | str=$1 23 | c=`echo $str | awk '{print toupper($0)}'` 24 | 25 | awk -vn=$1 -vN=$c -vp=$folder 'BEGIN{ 26 | hN = p"/"n".h"; 27 | print "Creating header: "n".h ..."; 28 | print "#ifndef "N"_H" > hN; 29 | print "#define "N"_H\n" > hN; 30 | print "#include " > hN; 31 | print "#include " > hN; 32 | print "#include " > hN; 33 | print "#include \"agent.h\"\n" > hN; 34 | print "class "n": public Agent" > hN; 35 | print "{" > hN; 36 | print "public:" > hN; 37 | print " "n"(int i, std::vector state, float tstep);" > hN; 38 | print " std::vector state_update(std::vector state);" > hN; 39 | print " void animation();" > hN; 40 | print "};\n" > hN; 41 | print "#endif /*"N"_H*/" >> hN; 42 | 43 | cN = p"/"n".cpp"; 44 | print "Creating file implementation: "n".cpp ..."; 45 | print "#include \""n".h\"" > cN; 46 | print "#include \"draw.h\"\n" > cN; 47 | print "using namespace std;\n" > cN; 48 | print n"::"n"(int i, vector s, float tstep)\n{" >> cN; 49 | print " state = s;\n ID = i;\n dt = tstep;\n orientation = 0.0;" > cN; 50 | print "}\n" >> cN; 51 | print "vector "n"::state_update(vector state)\n{ "> cN; 52 | print " float v_x, v_y;"> cN; 53 | print " controller->get_velocity_command(ID, v_x, v_y);" > cN; 54 | print " return state;" > cN; 55 | print " /*** Include your model here ***/ \n}\n" > cN; 56 | print "void "n"::animation()\n{" > cN 57 | print " draw d;" > cN 58 | print " /*** Draw your agent here. ***/" > cN; 59 | print " d.circle(param->scale());\n}" > cN 60 | 61 | cN = p"/README.md"; 62 | print "Creating README file"; 63 | print "#"n"" > cN; 64 | print "Please include a description of the agent here, both for yourself and for future users!" > cN; 65 | }' 66 | 67 | exit 0; 68 | -------------------------------------------------------------------------------- /sw/math/randomgenerator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * C++ Program to Check whether Undirected Graph is Connected using BFS 3 | * Code extracted from Sanfoundry Global Education & Learning Series 4 | * http://www.sanfoundry.com/cpp-program-check-undirected-graph-connected-bfs/ 5 | */ 6 | #ifndef RANDOM_GENERATOR_H 7 | #define RANDOM_GENERATOR_H 8 | 9 | #include "stdlib.h" 10 | #include "math.h" 11 | #include 12 | #include 13 | 14 | /** 15 | * Defines a graph structure that can be used to assess the topology of the swarm 16 | */ 17 | class random_generator 18 | { 19 | std::default_random_engine generator; 20 | 21 | public: 22 | 23 | /** 24 | * Constructor instatiates a list of all connections in the graph. 25 | */ 26 | random_generator(); 27 | 28 | /** 29 | * Get a random value of type float between a min and max 30 | * 31 | * @param min Minimum value in range 32 | * @param max Maximum value in range 33 | */ 34 | float uniform_float(float min, float max); 35 | 36 | /** 37 | * Get a random value of type int between a min and a max 38 | * 39 | * @param min Minimum value in range 40 | * @param max Maximum value in range 41 | */ 42 | int uniform_int(int min, int max); 43 | 44 | /** 45 | * Get a random value according to a Gaussian distribution 46 | * 47 | * @param mean Mean of the gaussian distribution 48 | * @param stddev Standard deviation 49 | */ 50 | float gaussian_float(float mean, float stddev); 51 | 52 | /** 53 | * Get a random bool value with probability p 54 | * 55 | * @param p Probability 56 | */ 57 | bool bernoulli(float p); 58 | 59 | int discrete_int(std::vector &d); 60 | 61 | /** 62 | * Generate a random vector with zero mean from a gaussian distribution 63 | * 64 | * @param length Desired length of the vector 65 | * @param mean Mean of the gaussian distribution 66 | * @param stddev Standard deviation 67 | */ 68 | std::vector gaussian_float_vector(const int &length, const float &mean, const float &std); 69 | 70 | /** 71 | * Generate a random vector with zero mean from a uniform distribution 72 | * 73 | * @param length Desired length of the vector 74 | * @param min Minimum value in range 75 | * @param max Maximum value in range 76 | */ 77 | std::vector uniform_float_vector(const int &length, const float &min, const float &max); 78 | 79 | }; 80 | 81 | 82 | #endif /* GRAPH_H */ 83 | 84 | -------------------------------------------------------------------------------- /conf/environments/rooms.txt: -------------------------------------------------------------------------------- 1 | 123.0 0.0 171.0 0.0 2 | 57.0 6.0 123.0 6.0 3 | 63.0 12.0 123.0 12.0 4 | 75.0 18.0 123.0 18.0 5 | 123.0 48.0 126.0 48.0 6 | 132.0 48.0 141.0 48.0 7 | 147.0 48.0 171.0 48.0 8 | 123.0 51.0 126.0 51.0 9 | 123.0 54.0 126.0 54.0 10 | 123.0 57.0 126.0 57.0 11 | 123.0 60.0 126.0 60.0 12 | 18.0 63.0 57.0 63.0 13 | 63.0 63.0 66.0 63.0 14 | 123.0 63.0 126.0 63.0 15 | 6.0 66.0 18.0 66.0 16 | 66.0 66.0 96.0 66.0 17 | 102.0 66.0 126.0 66.0 18 | 132.0 66.0 141.0 66.0 19 | 147.0 66.0 180.0 66.0 20 | 6.0 72.0 18.0 72.0 21 | 66.0 72.0 96.0 72.0 22 | 102.0 72.0 132.0 72.0 23 | 66.0 75.0 96.0 75.0 24 | 102.0 75.0 132.0 75.0 25 | 66.0 84.0 96.0 84.0 26 | 102.0 84.0 132.0 84.0 27 | 18.0 111.0 54.0 111.0 28 | 63.0 111.0 66.0 111.0 29 | 132.0 114.0 135.0 114.0 30 | 147.0 114.0 180.0 114.0 31 | 15.0 117.0 54.0 117.0 32 | 123.0 120.0 135.0 120.0 33 | 147.0 120.0 171.0 120.0 34 | 78.0 123.0 96.0 123.0 35 | 102.0 123.0 123.0 123.0 36 | 63.0 126.0 78.0 126.0 37 | 63.0 132.0 78.0 132.0 38 | 15.0 165.0 63.0 165.0 39 | 126.0 168.0 141.0 168.0 40 | 147.0 168.0 171.0 168.0 41 | 78.0 171.0 117.0 171.0 42 | 117.0 174.0 147.0 174.0 43 | 6.0 66.0 6.0 72.0 44 | 15.0 117.0 15.0 165.0 45 | 18.0 63.0 18.0 66.0 46 | 18.0 72.0 18.0 111.0 47 | 54.0 111.0 54.0 117.0 48 | 57.0 6.0 57.0 63.0 49 | 63.0 12.0 63.0 63.0 50 | 63.0 111.0 63.0 126.0 51 | 63.0 132.0 63.0 165.0 52 | 66.0 63.0 66.0 66.0 53 | 66.0 72.0 66.0 75.0 54 | 66.0 84.0 66.0 111.0 55 | 69.0 72.0 69.0 75.0 56 | 72.0 72.0 72.0 75.0 57 | 75.0 18.0 75.0 66.0 58 | 75.0 72.0 75.0 75.0 59 | 78.0 72.0 78.0 75.0 60 | 78.0 123.0 78.0 126.0 61 | 78.0 132.0 78.0 171.0 62 | 81.0 72.0 81.0 75.0 63 | 84.0 72.0 84.0 75.0 64 | 87.0 72.0 87.0 75.0 65 | 90.0 72.0 90.0 75.0 66 | 93.0 72.0 93.0 75.0 67 | 96.0 72.0 96.0 75.0 68 | 96.0 84.0 96.0 123.0 69 | 102.0 72.0 102.0 75.0 70 | 102.0 84.0 102.0 123.0 71 | 105.0 72.0 105.0 75.0 72 | 108.0 72.0 108.0 75.0 73 | 111.0 72.0 111.0 75.0 74 | 114.0 72.0 114.0 75.0 75 | 117.0 72.0 117.0 75.0 76 | 117.0 171.0 117.0 174.0 77 | 120.0 72.0 120.0 75.0 78 | 123.0 0.0 123.0 6.0 79 | 123.0 12.0 123.0 21.0 80 | 123.0 30.0 123.0 66.0 81 | 123.0 72.0 123.0 75.0 82 | 123.0 120.0 123.0 123.0 83 | 126.0 48.0 126.0 66.0 84 | 126.0 72.0 126.0 75.0 85 | 129.0 72.0 129.0 75.0 86 | 132.0 48.0 132.0 75.0 87 | 132.0 84.0 132.0 114.0 88 | 135.0 114.0 135.0 120.0 89 | 141.0 48.0 141.0 66.0 90 | 141.0 114.0 141.0 120.0 91 | 147.0 48.0 147.0 66.0 92 | 147.0 114.0 147.0 120.0 93 | 147.0 168.0 147.0 174.0 94 | 171.0 0.0 171.0 48.0 95 | 171.0 120.0 171.0 168.0 96 | 180.0 66.0 180.0 114.0 -------------------------------------------------------------------------------- /sw/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Swarmulator is a swarm simulation environment. 4 | * Its design purpose is to be a simple testing platform to observe the emergent behavior of a group of agents. 5 | * To program specific behaviors, you can do so in the controller.cpp/controller.h file. 6 | * 7 | * Mario Coppola, 2017-2020. 8 | * 9 | */ 10 | 11 | /** 12 | * Include standard and thread libraries 13 | */ 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | /** 21 | * Include top level threads 22 | */ 23 | #include "main.h" // Contains extern defines for global variables 24 | #include "simulation_thread.h" // Thread that handles the simulation 25 | #include "animation_thread.h" // Thread that handles animation 26 | #include "logger_thread.h" // Thread that handles the logger 27 | 28 | /** 29 | * Parameters from the XML file 30 | */ 31 | std::unique_ptr param(parameters("conf/parameters.xml", xml_schema::flags::dont_validate)); 32 | 33 | /** 34 | * Global variables used throughout simulation 35 | */ 36 | uint nagents; // Number of agents in the simulation 37 | std::vector s; // Set up the agents 38 | std::shared_mutex mtx; // Mutex needed to lock threads 39 | std::shared_mutex mtx_env; // Mutex needed to lock threads 40 | float realtimefactor; // Real time factor of simulation 41 | float simtime_seconds = 0; // Initial simulation time 42 | bool program_running = false; // Program running, initiated false until the beginning 43 | Environment environment; // Environment walls 44 | std::string identifier; // Log name identifier 45 | 46 | /** 47 | * The main function launches separate threads that control independent 48 | * functions of the code. All threads are designed to be optional with the 49 | * exception of the simulation thread. 50 | */ 51 | int main(int argc, char *argv[]) 52 | { 53 | program_running = true; // Program is running 54 | 55 | // Set FIFO identifier 56 | if (argc > 2) { // If supplied by the user, use the given input 57 | std::string s = ""; 58 | s += argv[2]; 59 | identifier = s; 60 | } else { // If not, use the current date and time 61 | identifier = currentDateTime(); 62 | } 63 | 64 | // Launch animation thread 65 | #ifdef ANIMATION 66 | std::thread animation(main_animation_thread); 67 | animation.detach(); 68 | #endif 69 | 70 | // Launch log thread 71 | #ifdef LOG 72 | std::thread logger(main_logger_thread); 73 | logger.detach(); 74 | #endif 75 | 76 | main_simulation_thread(argc, argv, identifier); 77 | 78 | // Exit 79 | terminalinfo::info_msg("Swarmulator exiting."); 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /sw/math/graph.cpp: -------------------------------------------------------------------------------- 1 | #include "graph.h" 2 | 3 | /** 4 | * Add Edge to connect v and w 5 | */ 6 | void Graph::addEdge(int v, int w) 7 | { 8 | adj[v].push_back(w); 9 | adj[w].push_back(v); 10 | } 11 | 12 | 13 | /** 14 | * Function that returns reverse (or transpose) of this graph 15 | */ 16 | Graph Graph::getTranspose() 17 | { 18 | Graph g(V); 19 | for (int v = 0; v < V; v++) { 20 | std::list::iterator i; 21 | for (i = adj[v].begin(); i != adj[v].end(); ++i) { 22 | g.adj[*i].push_back(v); 23 | } 24 | } 25 | return g; 26 | } 27 | 28 | /** 29 | * A recursive function to print BFS starting from s 30 | */ 31 | void Graph::BFS(int s, bool visited[]) 32 | { 33 | std::list q; 34 | std::list::iterator i; 35 | visited[s] = true; 36 | q.push_back(s); 37 | while (!q.empty()) { 38 | s = q.front(); 39 | q.pop_front(); 40 | for (i = adj[s].begin(); i != adj[s].end(); ++i) { 41 | if (!visited[*i]) { 42 | visited[*i] = true; 43 | q.push_back(*i); 44 | } 45 | } 46 | } 47 | } 48 | 49 | void Graph::DFS(int v, bool visited[]) 50 | { 51 | // Mark the current node as visited and print it 52 | visited[v] = true; 53 | 54 | // Recur for all the vertices 55 | // adjacent to this vertex 56 | std::list::iterator i; 57 | for (i = adj[v].begin(); i != adj[v].end(); ++i) 58 | if (!visited[*i]) { 59 | DFS(*i, visited); 60 | } 61 | } 62 | 63 | // Method to print connected components in an 64 | // undirected graph 65 | uint Graph::connectedComponents() 66 | { 67 | // Mark all the vertices as not visited 68 | bool *visited = new bool[V]; 69 | for (int v = 0; v < V; v++) { 70 | visited[v] = false; 71 | } 72 | uint count = 0; 73 | for (int v = 0; v < V; v++) { 74 | if (visited[v] == false) { 75 | // print all reachable vertices 76 | // from v 77 | DFS(v, visited); 78 | count++; 79 | } 80 | } 81 | delete[] visited; 82 | 83 | return count; 84 | } 85 | 86 | /** 87 | * Check if Graph is Connected 88 | */ 89 | bool Graph::isConnected() 90 | { 91 | bool visited[V]; 92 | for (int i = 0; i < V; i++) { 93 | visited[i] = false; 94 | } 95 | 96 | BFS(0, visited); 97 | for (int i = 0; i < V; i++) 98 | if (visited[i] == false) { 99 | return false; 100 | } 101 | 102 | Graph gr = getTranspose(); 103 | 104 | for (int i = 0; i < V; i++) { 105 | visited[i] = false; 106 | } 107 | 108 | gr.BFS(0, visited); 109 | for (int i = 0; i < V; i++) 110 | if (visited[i] == false) { 111 | return false; 112 | } 113 | 114 | return true; 115 | } 116 | -------------------------------------------------------------------------------- /sw/simulation/controllers/forage/forage.cpp: -------------------------------------------------------------------------------- 1 | #include "forage.h" 2 | #include "agent.h" 3 | #include "main.h" 4 | #include "randomgenerator.h" 5 | #include "auxiliary.h" 6 | #include "math.h" 7 | #include "draw.h" 8 | 9 | using namespace std; 10 | #define SENSOR_MAX_RANGE 1.8 11 | 12 | forage::forage() : Controller() 13 | { 14 | // Initial values 15 | timer = rg.uniform_int(0, timelim); 16 | choose = false; 17 | holds_food = false; 18 | v_x_ref = 0.0; 19 | v_y_ref = 0.0; 20 | 21 | // Control values 22 | timelim = 10.0 * param->simulation_updatefreq(); 23 | vmean = 0.5; 24 | 25 | // Load policy 26 | if (!strcmp(param->policy().c_str(), "")) { motion_p.assign(16, 1.0); } 27 | else { motion_p = read_array(param->policy()); } 28 | } 29 | 30 | void forage::get_velocity_command(const uint16_t ID, float &v_x, float &psi_rate) 31 | { 32 | v_x = 0; 33 | psi_rate = 0; 34 | float temp, br; 35 | get_lattice_motion_range(ID, v_x, temp, SENSOR_MAX_RANGE); // Repulsion from neighbors 36 | 37 | // Make the choice. To explore or not to explore? 38 | if (!choose) { 39 | choose = true; 40 | state = environment.nest; // +8 to center around 0 41 | keepbounded(state, 0, 15); 42 | st = int(state); 43 | if (rg.bernoulli(1.0 - motion_p[st])) { explore = false; } 44 | else { explore = true; } //environment.eat_food(0.1); } 45 | } 46 | 47 | // Behavior 48 | if (explore) { 49 | if (timer == 1) { // Go explore, change direction every new timer 50 | v_x_ref = vmean; 51 | v_y_ref = wrapToPi_f(rg.gaussian_float(0., 0.2)); 52 | } 53 | 54 | uint16_t ID_food; // for sim purposes, used to delete the correct food item once grabbed 55 | if (holds_food) { 56 | o.beacon(ID, br, v_y_ref); // get distance + angle to beacon 57 | v_x_ref = br; 58 | v_y_ref = 0.5 * wrapToPi_f(v_y_ref); // gain on control 59 | if (br < SENSOR_MAX_RANGE) { // Drop the food if you are in the vicinity of the nest 60 | environment.drop_food(); 61 | holds_food = false; 62 | choose = false; 63 | timer = 1; // reset timer 64 | } 65 | } else if (o.sense_food(ID, ID_food, SENSOR_MAX_RANGE)) { 66 | environment.grab_food(ID_food); // Grab the food item ID_food 67 | holds_food = true; 68 | } 69 | } else { // don't explore 70 | v_x_ref = 0.0; 71 | v_y_ref = 0.0; 72 | if (timer == 1) {choose = false;} 73 | } 74 | increase_counter_to_value(timer, timelim, 1); 75 | v_x += v_x_ref; 76 | psi_rate += v_y_ref; 77 | wall_avoidance_turn(ID, v_x, psi_rate, 2.5); // Can see walls at 2.5 m way 78 | } 79 | 80 | void forage::animation(const uint16_t ID) 81 | { 82 | draw d; 83 | d.circle_loop(SENSOR_MAX_RANGE); 84 | } -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/robots/robotConditions.cpp: -------------------------------------------------------------------------------- 1 | #include "condition.h" 2 | 3 | #include "less_than.h" 4 | #include "greater_than.h" 5 | 6 | namespace BT 7 | { 8 | 9 | std::tuple input(int i) 10 | { 11 | char buff[255]; 12 | double threshold; 13 | double lower_lim; 14 | double upper_lim; 15 | 16 | if (i < KCOND) { 17 | sprintf(buff, "sensor%d", i); 18 | lower_lim = 0.; 19 | upper_lim = 8; 20 | } else if (i < NUMBER_OF_VARS) { 21 | sprintf(buff, "wheelSpeed%d", i - KCOND); 22 | lower_lim = 0; 23 | upper_lim = 1.0; 24 | } else { 25 | std::cerr << "Check number of variables, requested: " << i << std::endl; 26 | sprintf(buff, "sensor"); 27 | lower_lim = 0.; 28 | upper_lim = 8; 29 | } 30 | 31 | threshold = (upper_lim - lower_lim) * (rand() % 101) / 100. + 32 | lower_lim; // test for 100 discrete points on range [lower_lim, upper_lim] 33 | 34 | return std::make_tuple(buff, threshold, lower_lim, upper_lim); 35 | } 36 | 37 | // *************************************************************** 38 | // *************************************************************** 39 | // Add all conditions to the if else if list below 40 | 41 | node *getCondition(std::string condition, size_t var) 42 | { 43 | node *task; 44 | if (condition.compare("less_than") == 0) { 45 | task = (node *) new less_than("robot", var); 46 | } else if 47 | (condition.compare("greater_than") == 0) { 48 | task = (node *) new greater_than("robot", var); 49 | } else { 50 | task = NULL; 51 | std::cerr << "Something is really wrong in :BT::node* getCondition(std::string condition)" << std::endl; 52 | } 53 | 54 | return task; 55 | } 56 | 57 | node *getCondition(std::string condition, std::vector inputs) 58 | { 59 | node *task; 60 | if (condition.compare("less_than") == 0) { 61 | task = (node *) new less_than("robot", static_cast(inputs[0]), inputs[1]); 62 | } else if (condition.compare("greater_than") == 0) { 63 | task = (node *) new greater_than("robot", static_cast(inputs[0]), inputs[1]); 64 | } else { 65 | task = NULL; 66 | std::cerr << "Something is really wrong in :BT::node* getCondition(std::string condition, std::vector* inputs)" 67 | << std::endl; 68 | } 69 | 70 | return task; 71 | } 72 | 73 | // Add all conditions to the if else if list below 74 | node *getCondition(size_t func /*= rand() % KCOND*/, size_t var /*= MAX_SIZE*/) 75 | { 76 | node *task; 77 | switch (func) { 78 | case 0: 79 | task = (node *) new greater_than("robot", var); 80 | break; 81 | case 1: 82 | task = (node *) new less_than("robot", var); 83 | break; 84 | default: 85 | std::cerr << "ERROR in getCondition(unsigned int func): number of conditions out of bounds" << std::endl; 86 | } 87 | return task; 88 | } 89 | 90 | } 91 | -------------------------------------------------------------------------------- /sw/simulation/environment.h: -------------------------------------------------------------------------------- 1 | #ifndef ENVIRONMENT_H 2 | #define ENVIRONMENT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "randomgenerator.h" 8 | 9 | class Environment 10 | { 11 | std::vector> walls; 12 | random_generator rg; 13 | public: 14 | std::vector> food; 15 | std::vector beacon; 16 | float nest; 17 | 18 | /** 19 | * @brief Construct a new Environment object 20 | * 21 | */ 22 | Environment(); 23 | 24 | /** 25 | * @brief Destroy the Environment object 26 | * 27 | */ 28 | ~Environment() {}; 29 | 30 | /** 31 | * @brief Define the initial obstacle list according to the list in conf/environments/.txt 32 | * You can indicate obstacle list in the conf/parameters.xml file, under 33 | * Make sure the file exists! 34 | */ 35 | void define_walls(void); 36 | 37 | /** 38 | * @brief Define the initial obstacle list according to the list in conf/environments/.txt 39 | * You can indicate obstacle list in the conf/parameters.xml file, under 40 | * Make sure the file exists! 41 | */ 42 | void define_food(uint64_t n); 43 | 44 | 45 | void define_beacon(float x, float y); 46 | 47 | /** 48 | * @brief Returns a point within the environment. 49 | * 50 | * @return float 51 | */ 52 | std::vector start(void); 53 | 54 | /** 55 | * @brief Returns the furthers point from (0,0) in the environment, used for initialization so that 56 | * the robots can be initialized in the same spot. 57 | * 58 | * @return float 59 | */ 60 | float limits(void); 61 | 62 | /** 63 | * @brief Add a new wall to the list, going from (x0,y0) to (x1,y1). 64 | * This is used to interactively create walls in the animation, just right click with the mouse! 65 | * 66 | * @param x0 Initial x 67 | * @param y0 Initial y 68 | * @param x1 Final x 69 | * @param y1 Final y 70 | */ 71 | void add_wall(float x0, float y0, float x1, float y1); 72 | 73 | /** 74 | * Senses whether the next state will go through a wall 75 | * 76 | * @param ID Robot to consider 77 | * @param s_n Next state 78 | * @param s Current state 79 | * @return true if the lines intersect, meaning that it will go through a wall, so that we can handle it. 80 | * @return false if the lines do not intersect, so that the robot will not go through a wall and can act normally. 81 | */ 82 | bool sensor(const uint16_t ID, std::vector s_n, std::vector s, float &angle); 83 | bool valid(const uint16_t ID, std::vector s_n, std::vector s); 84 | 85 | void grab_food(uint64_t food_ID); 86 | void drop_food(); 87 | void eat_food(float); 88 | 89 | void loop(void); 90 | 91 | /** 92 | * Function used to draw all the walls in the animation. It is called by the animation thread. 93 | */ 94 | void animate(void); 95 | }; 96 | 97 | #endif /*ENVIRONMENT_H*/ 98 | -------------------------------------------------------------------------------- /sw/simulation/controllers/pattern_formation/pattern_formation.cpp: -------------------------------------------------------------------------------- 1 | #include "pattern_formation.h" 2 | #include // std::find 3 | #include "agent.h" 4 | #include "main.h" 5 | #include "randomgenerator.h" 6 | #include "trigonometry.h" 7 | #include "auxiliary.h" 8 | #include "draw.h" 9 | 10 | using namespace std; 11 | 12 | pattern_formation::pattern_formation() : lattice_basic() 13 | { 14 | if (!strcmp(param->policy().c_str(), "")) { 15 | terminalinfo::error_msg("Please specify a valid policy file"); 16 | } else { 17 | t.set_state_action_matrix(param->policy()); 18 | } 19 | 20 | moving_timer = 0; 21 | beta_des.push_back(0.0); 22 | beta_des.push_back(atan(_ddes_y / _ddes_x)); 23 | beta_des.push_back(M_PI / 2.0); 24 | beta_des.push_back(M_PI / 2.0 + atan(_ddes_x / _ddes_y)); 25 | _kr = 1.0; // Repulsion gain 26 | _ka = 5.0; // Attraction gain, overwrite from parent class if you want strong attraction 27 | } 28 | 29 | void pattern_formation::get_velocity_command(const uint16_t ID, float &v_x, float &v_y) 30 | { 31 | v_x = 0.0; 32 | v_y = 0.0; 33 | 34 | float timelim = 1.3 * param->simulation_updatefreq(); 35 | float tadj = timelim * 2.0; // This is actually time for action + t_adj, thus t_adj = time for action 36 | float twait = tadj * 2.0; // This is actually time for action + t_adj + t_wait 37 | 38 | // Initialize local moving_timer with random variable 39 | if (moving_timer == 0) { 40 | moving_timer = rand() % (int)twait; 41 | } 42 | 43 | vector state(8, 0); 44 | vector state_ID; 45 | 46 | // The ID is just used for simulation purposes 47 | t.assess_situation(ID, state, state_ID); 48 | int state_index = bool2int(state); 49 | state_index = bool2int(state); 50 | 51 | // Get vector of all neighbors from closest to furthest 52 | vector closest = o.request_closest(ID); 53 | 54 | // Can I move or are my neighbors moving? 55 | bool canImove = check_motion(state_ID); 56 | if (!canImove) { 57 | selected_action = -2; // Reset actions 58 | moving_timer = tadj; // Reset moving timer 59 | } 60 | 61 | // Try to find an action that suits the state, if available (otherwise you are in Sdes or Sblocked) 62 | // If you are already busy with an action, then don't change the action 63 | std::map>::iterator state_action_row; 64 | state_action_row = t.state_action_matrix.find(state_index); 65 | if (!o.see_if_moving(ID) && state_action_row != t.state_action_matrix.end()) { 66 | selected_action = *select_randomly(state_action_row->second.begin(), state_action_row->second.end()); 67 | // selected_action = wraptosequence(selected_action + 1 + rot, 1, 8) - 1; // North correction 68 | } else if (!o.see_if_moving(ID)) { 69 | selected_action = -2; 70 | } 71 | 72 | // Controller 73 | moving = false; 74 | if (canImove) { 75 | if (selected_action > -1 && moving_timer < timelim) { 76 | actionmotion(selected_action, v_x, v_y); 77 | moving = true; 78 | } else { 79 | get_lattice_motion_all(ID, state_ID, closest, v_x, v_y); 80 | } 81 | increase_counter(moving_timer, twait); 82 | } 83 | 84 | } 85 | 86 | void pattern_formation::animation(const uint16_t ID) 87 | { 88 | draw d; 89 | d.circle_loop(sensor_range); 90 | } -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/behavior_tree.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_tree.h" 2 | #include "draw.h" 3 | #include "terminalinfo.h" 4 | #include "auxiliary.h" 5 | 6 | #define SENSOR_MAX_RANGE 1.8 7 | #define WALL_SENSOR 2.5 8 | using namespace std; 9 | 10 | void behavior_tree::lattice_all(const int &ID, const vector &closest, float &v_x, float &v_y) 11 | { 12 | if (!closest.empty()) { 13 | for (size_t i = 0; i < closest.size(); i++) { 14 | get_lattice_motion(ID, closest[i], v_x, v_y); 15 | } 16 | v_x = v_x / (float)closest.size(); 17 | v_y = v_y / (float)closest.size(); 18 | } 19 | } 20 | 21 | 22 | behavior_tree::behavior_tree() : Controller() 23 | { 24 | // Load the behavior tree 25 | tree = loadFile(param->policy().c_str()); 26 | 27 | v_x_ref = rg.gaussian_float(0.0, 1.0); 28 | v_y_ref = rg.gaussian_float(0.0, 1.0); 29 | 30 | // Initialize input sensors 31 | BLKB.set("sensor0", 0.); // Sensor 0 32 | 33 | // Initialize outputs 34 | BLKB.set("wheelSpeed0", 0.); // Output 0 35 | BLKB.set("wheelSpeed1", 0.); // Output 1 36 | 37 | moving_timer = 0; 38 | } 39 | 40 | void behavior_tree::get_velocity_command(const uint16_t ID, float &v_x, float &v_y) 41 | { 42 | v_x = 0; 43 | v_y = 0; 44 | 45 | float timelim = 2.0 * param->simulation_updatefreq(); 46 | 47 | // Get vector of all neighbors from closest to furthest 48 | 49 | vector closest = o.request_closest_inrange(ID, SENSOR_MAX_RANGE); 50 | lattice_all(ID, closest, v_x, v_y); 51 | 52 | float vmean = 1.0; 53 | 54 | /**** Step 1 of 3: Set current state according to sensors ****/ 55 | BLKB.set("sensor0", closest.size()); 56 | 57 | /**** Step 2 of 3: Tick the tree based on the current state ****/ 58 | tree->tick(&BLKB); 59 | 60 | /**** Step 3 of 3: Set outputs (do this once, else keep!) ****/ 61 | float p_motion = BLKB.get("wheelSpeed0"); 62 | 63 | string d = "p=" + to_string(p_motion); 64 | terminalinfo::debug_msg(d, ID); // Debug 65 | 66 | /******** Probabilistic aggregation behavior ***********/ 67 | // Initialize local moving_timer with random variable 68 | if (moving_timer == 0) { 69 | moving_timer = rg.uniform_int(0, timelim); 70 | } 71 | // Behavior 72 | if (moving_timer == 1) { 73 | if (rg.bernoulli(1.0 - p_motion)) { 74 | v_x_ref = 0.0; 75 | v_y_ref = 0.0; 76 | moving = false; 77 | } else { // Else explore randomly, change heading 78 | ang = rg.uniform_float(0.0, 2 * M_PI); 79 | if (moving) { 80 | float ext = rg.gaussian_float(0.0, 0.5); 81 | float temp; 82 | cart2polar(v_x_ref, v_y_ref, temp, ang); 83 | ang += ext; 84 | } 85 | wrapTo2Pi(ang); 86 | polar2cart(vmean, ang, v_x_ref, v_y_ref); 87 | moving = true; 88 | } 89 | } 90 | increase_counter(moving_timer, timelim); 91 | /*******************************************************/ 92 | 93 | wall_avoidance_bounce(ID, v_x_ref, v_y_ref, SENSOR_MAX_RANGE); 94 | 95 | v_x += v_x_ref; 96 | v_y += v_y_ref; 97 | 98 | d = to_string(v_x) + ", " + to_string(v_y); 99 | terminalinfo::debug_msg(d, ID); 100 | } 101 | 102 | 103 | void behavior_tree::animation(const uint16_t ID) 104 | { 105 | draw d; 106 | d.circle_loop(SENSOR_MAX_RANGE); 107 | } -------------------------------------------------------------------------------- /sw/simulation/controllers/pattern_formation/lattice_basic.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_LATTICE_BASIC_H 2 | #define CONTROLLER_LATTICE_BASIC_H 3 | 4 | #include "controller.h" 5 | #include "template_calculator.h" 6 | 7 | /** 8 | * Controller that holds a basic lattice between particles by handling attraction and 9 | * repulsion forces 10 | * This class is meant to be used as a parent class for any higher level controller 11 | * that wants to hold a lattice at all times, as is the case for the pattern formation 12 | * controller for instance. 13 | */ 14 | class lattice_basic : public Controller 15 | { 16 | public: 17 | Template_Calculator t; 18 | std::vector beta_des; 19 | float _v_adj; // Adjustment velocity 20 | float d_safe; // Safety distance after which only nearest neighbor is considered 21 | float sensor_range; 22 | 23 | /** 24 | * @brief Construct a new lattice_basic object 25 | */ 26 | lattice_basic(); 27 | 28 | /** 29 | * @brief Destroy the lattice basic object 30 | * 31 | */ 32 | ~lattice_basic() {}; 33 | 34 | /** 35 | * @brief 36 | * 37 | * @param v_r 38 | * @param v_b 39 | * @param v_x 40 | * @param v_y 41 | */ 42 | void attractionmotion(const float &v_r, const float &v_b, float &v_x, float &v_y); 43 | 44 | /** 45 | * @brief Lattice motion 46 | * 47 | * @param v_r 48 | * @param v_adj 49 | * @param v_b 50 | * @param bdes 51 | * @param v_x 52 | * @param v_y 53 | */ 54 | void latticemotion(const float &v_r, const float &v_adj, const float &v_b, const float &bdes, float &v_x, float &v_y); 55 | 56 | /** 57 | * @brief Attraction force 58 | * 59 | * @param u 60 | * @param b_eq 61 | * @return float 62 | */ 63 | float f_attraction(const float &u, const float &b_eq); 64 | 65 | /** 66 | * @brief Get the attraction velocity object 67 | * 68 | * @param u 69 | * @param b_eq 70 | * @return float 71 | */ 72 | float get_attraction_velocity(const float &u, const float &b_eq); 73 | 74 | /** 75 | * @brief Move along the lattice 76 | * 77 | * @param selected_action 78 | * @param v_x 79 | * @param v_y 80 | */ 81 | void actionmotion(const int &selected_action, float &v_x, float &v_y); 82 | 83 | /** 84 | * Determine if neighbors are moving 85 | * 86 | * @param state_ID 87 | * @return true 88 | * @return false 89 | */ 90 | bool check_motion(const std::vector &state_ID); 91 | 92 | /** 93 | * @brief Get the lattice motion object 94 | * 95 | * @param ID 96 | * @param state_ID 97 | * @param v_x 98 | * @param v_y 99 | */ 100 | void get_lattice_motion(const int &ID, const int &state_ID, float &v_x, float &v_y); 101 | 102 | /** 103 | * @brief Handles all the above in one command, it can be called by the child controller 104 | * to handle the lower level lattice control (see pattern formation controller for an example) 105 | * 106 | * @param ID 107 | * @param state_ID 108 | * @param closest 109 | * @param v_x 110 | * @param v_y 111 | */ 112 | void get_lattice_motion_all(const int &ID, const std::vector &state_ID, const std::vector &closest, 113 | float &v_x, float &v_y); 114 | 115 | /** 116 | * @brief Get the velocity command object. Not implemented here, but by child controllers of this class. 117 | * 118 | * @param ID 119 | * @param v_x 120 | * @param v_y 121 | */ 122 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y) = 0; 123 | }; 124 | 125 | #endif /*CONTROLLER_LATTICE_BASIC_H*/ 126 | -------------------------------------------------------------------------------- /sw/animation/animation_thread.h: -------------------------------------------------------------------------------- 1 | #ifndef ANIMATION_THREAD_H 2 | #define ANIMATION_THREAD_H 3 | 4 | #include 5 | 6 | #include "main.h" 7 | #include "draw.h" 8 | #include "user_interaction.h" 9 | #include "terminalinfo.h" 10 | #include "trigonometry.h" 11 | 12 | bool animation_running = false; 13 | /** 14 | * Main animation loop. 15 | * Takes care of drawing the agents in their corrective location. 16 | */ 17 | void main_loop_function() 18 | { 19 | if (!animation_running) { 20 | terminalinfo::info_msg("Animation started."); 21 | animation_running = true; 22 | } 23 | 24 | // Add depth (used internally to block obstructed objects) 25 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 26 | glLoadIdentity(); 27 | 28 | // Get current window size w.r.t. beginning 29 | xrat = (float)param->window_width() / (float)glutGet(GLUT_WINDOW_WIDTH); 30 | yrat = (float)param->window_height() / (float)glutGet(GLUT_WINDOW_HEIGHT); 31 | 32 | zoom_scale = -(float)10 / (-(float)10 + (float)zoom); 33 | glTranslatef(center_x, center_y, -10 + zoom); 34 | 35 | // Draw fixed one time objects 36 | static draw drawer; // Drawer object 37 | drawer.data(); // Put data in corner 38 | drawer.axes(); // Put x and y global axes 39 | drawer.axis_label(); // Axis label 40 | environment.animate(); // Animate the environment walls 41 | 42 | // Draw all robots 43 | uint r = s.size(); 44 | if (r > 0) { 45 | for (uint16_t ID = 0; ID < r; ID++) { 46 | // Input: ID, p_x global, p_y global, orientation global 47 | drawer.agent(ID, s[ID]->state.at(0), s[ID]->state.at(1), s[ID]->orientation); 48 | // Input: ID, p_x global, p_y global, v_x global, v_y global 49 | drawer.velocity_arrow(ID, s[ID]->state.at(0), s[ID]->state.at(1), s[ID]->state.at(2), s[ID]->state.at(3)); 50 | } 51 | } 52 | 53 | // Swap buffers (color buffers, makes previous render visible) 54 | glutSwapBuffers(); 55 | 56 | user_interaction(); // Activate interactive functions (mouse + keyboard), important: use this before draw functions! 57 | if (!program_running) {std::terminate();} 58 | } 59 | 60 | /** 61 | * Initialze OpenGL perspective matrix 62 | * 63 | * @param width Width of the animation window 64 | * @param height Height of the animation window 65 | */ 66 | void GL_Setup(int width, int height) 67 | { 68 | glViewport(0, 100, width, height); 69 | glMatrixMode(GL_PROJECTION); 70 | gluPerspective(45, (float)width / height, .1, 100); 71 | glMatrixMode(GL_MODELVIEW); 72 | } 73 | 74 | /** 75 | * Thread function that initiates the animation 76 | */ 77 | void main_animation_thread() 78 | { 79 | // Initialize all variables declared in drawingparams.h 80 | center_x = 0; 81 | center_y = 0; 82 | sx = 0; 83 | sy = 0; 84 | zoom = param->zoom(); // From parameters file 85 | zoom_scale = 0; 86 | pointer_x = 0; 87 | pointer_y = 0; 88 | paused = false; 89 | xrat = 1.0; 90 | yrat = 1.0; 91 | 92 | // Set up simulation window 93 | int argc = 1; 94 | char *argv[1] = {(char *)" "}; 95 | glutInit(&argc, argv); 96 | glutInitWindowPosition(0, 0); // Initialize at top left corner of screen 97 | glutInitWindowSize(param->window_width(), param->window_height()); // Set dimensions 98 | glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE); 99 | glutCreateWindow("Swarmulator"); // Set window title to Swarmulator 100 | glutIdleFunc(main_loop_function); // Set main loop 101 | GL_Setup(param->window_width(), param->window_height()); // Set up window parameters 102 | glutMainLoop(); // Initiate main drawing loop 103 | } 104 | 105 | #endif /*ANIMATION_THREAD_H*/ 106 | -------------------------------------------------------------------------------- /sw/animation/draw.h: -------------------------------------------------------------------------------- 1 | #ifndef DRAW_H 2 | #define DRAW_H 3 | 4 | #include 5 | #include 6 | #include // std::stringstream 7 | #include 8 | 9 | #include "main.h" 10 | #include "drawingparams.h" 11 | #include "settings.h" 12 | 13 | /** 14 | * @brief Object that takes care of OpenGL drawing for all elements of interest. 15 | * This class contains general openGL function that 16 | * can be used to animate and visualize the swarm at runtime. 17 | * It is launched and managed by the animation thread. 18 | */ 19 | class draw 20 | { 21 | public: 22 | /******* General openGL functions ********/ 23 | 24 | /** 25 | * Draw a simple triangle of size scl 26 | * 27 | * @param s Scale of the triangle 28 | */ 29 | void triangle(const float &s); 30 | 31 | /** 32 | * Draw a red circle of radius r 33 | * 34 | * @param r Radius of the circle 35 | */ 36 | void circle(const float &r); 37 | 38 | /** 39 | * Draw a white unfilled circular loop 40 | * 41 | * @param r Radius of the circular loop 42 | */ 43 | void circle_loop(const float &r); 44 | 45 | /** 46 | * Draw a white line from (0,0) to (x,y) 47 | * 48 | * @param x x position (East) 49 | * @param y y position (North) 50 | */ 51 | void line(const float &x, const float &y); 52 | 53 | /** 54 | * @brief Draw a white line with a specified width from (0,0) to (x,y) 55 | * 56 | * @param x 57 | * @param y 58 | * @param width 59 | */ 60 | void line(const float &x, const float &y, const float &width); 61 | 62 | /** 63 | * Draw the global x and y axes at (0,0) 64 | */ 65 | void axes(); 66 | 67 | /** 68 | * Draw a segment (used for walls and obstacles) 69 | */ 70 | void segment(const float &x0, const float &y0, const float &x1, const float &y1); 71 | 72 | /** 73 | * Draw a small white point 74 | */ 75 | void point(); 76 | 77 | /** 78 | * Draw relevant simulation data in the bottom left corner (like the time of simulation) 79 | */ 80 | void data(); 81 | 82 | /** 83 | * Write the x,y label on the global axis along the given dimension dim 84 | * 85 | * @param dim Specifies (=0) for writing the x label and (=1) for writing the y label 86 | */ 87 | void axis_label(); 88 | 89 | /** 90 | * Draw the ID number of an agent on top of the agent 91 | * 92 | * @param ID The ID of the robot in question 93 | */ 94 | void agent_number(const uint16_t &ID); 95 | 96 | /** 97 | * Draw the agent (uses internal function defined by the agent class) 98 | * 99 | * @param ID The ID of the robot in question 100 | * @param x The x position of the robot 101 | * @param y The y position of the robot 102 | * @param orientation The orientation of the robot 103 | */ 104 | void agent(const uint16_t &ID, const float &x, const float &y, const float &orientation); 105 | 106 | /** 107 | * Draw a line showing the velocity of the agent 108 | * 109 | * @param ID The ID of the robot in question 110 | * @param x The x position of the robot (East) 111 | * @param y The y position of the robot (North) 112 | * @param v_x The velocity of the robot in x (global) 113 | * @param v_y The velocity of the robot in y (global) 114 | */ 115 | void velocity_arrow(const uint16_t &ID, const float &x, const float &y, const float &v_x, const float &v_y); 116 | 117 | /** 118 | * Draw a gray dot indicating food (or something else if you like) at position (x,y) 119 | * 120 | * @param x The x position of the food 121 | * @param y The y position of the food 122 | */ 123 | void food(const float &x, const float &y); 124 | }; 125 | 126 | #endif /*DRAW_H*/ 127 | -------------------------------------------------------------------------------- /scripts/python/evolution_example.py: -------------------------------------------------------------------------------- 1 | import random, sys, pickle, argparse 2 | import numpy as np 3 | from tools import fileHandler as fh 4 | from classes import evolution, swarmulator 5 | 6 | ## Run as 7 | # python3 main_standard_evolution.py CONTROLLER AGENT 8 | # Example: 9 | # python3 main_standard_evolution.py aggregation particle 10 | 11 | ##################### 12 | # Argument parser # 13 | ##################### 14 | parser = argparse.ArgumentParser(description='Evolve a controller using swarmulator') 15 | parser.add_argument('controller', type=str, help="(str) Controller to use") 16 | parser.add_argument('agent', type=str, help="(str) Swramulator agent to use") 17 | parser.add_argument('-gen', type=int, help="(int) Max number generations, default = 100", default=100) 18 | parser.add_argument('-batchsize', type=int, help="(int) Batch size. How many parallel tests to try, default = 5", default=5) 19 | parser.add_argument('-resume', type=str, help="(str) Resume after quitting from the indicated saved file, default = None", default=None) 20 | parser.add_argument('-plot', type=str, help="(str) If set, it will plot the evolution from a saved run, default = None", default=None) 21 | parser.add_argument('-id', type=int, help="(int) Evolutionary run ID, default = 1", default=1) 22 | args = parser.parse_args() 23 | 24 | ########################## 25 | # Load Swarmulator API # 26 | ########################## 27 | print("Loading and building Swarmulator") 28 | sim = swarmulator.swarmulator(verbose=False) 29 | sim.make(controller=args.controller, agent=args.agent, clean=True, logger=False, verbose=False) 30 | # Swarmulator settings 31 | sim.runtime_setting("time_limit", str("100")) # Time limit of each simulation 32 | sim.runtime_setting("simulation_realtimefactor", str("300")) # Real time factor 33 | sim.runtime_setting("environment", "square") # Environment, leave empty for boundless 34 | sim.runtime_setting("fitness", "aggregation_clusters") # Fitness function to use (in sw/simulation/fitness_functions.h) 35 | filename = "evo_run_%s_%s_%i" % (args.controller, args.agent, args.id) 36 | print("This run will save at every new generation in the file %s.pkl" % filename) 37 | print("If you want to resume, please load it using the -resume input option.") 38 | 39 | ###################### 40 | # Fitness function # 41 | ###################### 42 | def fitness(individual): 43 | ### Set the policy file that swarmulator reads 44 | policy_file = "conf/policies/policy_evolved_temp.txt" 45 | fh.save_to_txt(individual, sim.path+policy_file) 46 | sim.runtime_setting("policy", policy_file) # Use random policy 47 | 48 | ### Run swarmulator in batches 49 | # Indicate the minimum number and the maximum number of agents to run with. 50 | # In this example, a run will feature anything between 10 and 20 robots. 51 | f = sim.batch_run((10,20),args.batchsize) # Run with 10-20 robots, 5 times (default args.batchsize=5) 52 | print(f) # Print the fitness 53 | return f.mean(), # Fitness = average (note trailing comma to cast to tuple!) 54 | 55 | ######################## 56 | # Load evolution API # 57 | ######################## 58 | e = evolution.evolution() 59 | # Specify the genome length and the population size 60 | e.setup(fitness, GENOME_LENGTH=8, POPULATION_SIZE=100) 61 | 62 | # Do not evolve, but only plot an evolution file as specified in args.plot 63 | if args.plot is not None: 64 | e.load(args.plot) 65 | e.plot_evolution() 66 | 67 | # Resume evolution from file args.resume 68 | elif args.resume is not None: 69 | e.load(args.resume) 70 | p = e.evolve(verbose=True, generations=args.gen, checkpoint=filename, population=e.pop) 71 | 72 | # Just run normally from the beginning 73 | else: 74 | p = e.evolve(verbose=True, generations=args.gen, checkpoint=filename) 75 | 76 | # Save 77 | e.save(filename) 78 | -------------------------------------------------------------------------------- /makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Swarmulator 2 | # 3 | # Mario Coppola, 2017-2018 4 | 5 | TARGET = swarmulator # application name 6 | BUILD_FOLDER = build 7 | SRC_FOLDER = sw 8 | 9 | CONTROLLER?=aggregation 10 | AGENT?=particle 11 | 12 | CONTROLLER_INCLUDE=\"$(CONTROLLER).h\" 13 | AGENT_INCLUDE=\"$(AGENT).h\" 14 | # Compiler parameters 15 | # -g adds debugging information to the executable file 16 | # -Wall turns on most, but not all, compiler warnings 17 | 18 | CC = g++ # chosen compiler 19 | CFLAGS = -g -Wall -std=gnu++17 -D_GLIBCXX_USE_NANOSLEEP -DSWARMULATOR -DCONTROLLER=$(CONTROLLER) -DAGENT=$(AGENT) -DAGENT_INCLUDE=$(AGENT_INCLUDE) -DCONTROLLER_INCLUDE=$(CONTROLLER_INCLUDE) -D_GLIBCXX_USE_CXX11_ABI=0 20 | OPT=-lglut -lGLU -lGL -lpthread -lxerces-c -Wno-deprecated-declarations -fno-inline-functions -lprotobuf 21 | 22 | ifeq ($(VERBOSE),ON) 23 | CFLAGS += -DVERBOSE 24 | endif 25 | 26 | ifeq ($(ANIMATION),ON) 27 | CFLAGS += -DANIMATION 28 | endif 29 | 30 | ifeq ($(LOG),ON) 31 | CFLAGS += -DLOG 32 | endif 33 | 34 | CTRL_FOLDER = sw/simulation/controllers 35 | AGNT_FOLDER = sw/simulation/agents 36 | 37 | # General parameters to include all relevant cpp files and all subfolders 38 | # INC_DIRS = $(shell find $(SRC_FOLDER) -path $(CTRL_FOLDER) -prune -o -type d) 39 | INC_DIRS = $(shell find $(SRC_FOLDER) -type d) 40 | SOURCES_CPP = $(shell find $(SRC_FOLDER) -path $(CTRL_FOLDER) -prune -o -path $(AGNT_FOLDER) -prune -o -name *.cpp -print) # Recursively find all cpp/c files 41 | SOURCES_C = $(shell find $(SRC_FOLDER) -path $(CTRL_FOLDER) -prune -o -path $(AGNT_FOLDER) -prune -o -name *.c -print) # Recursively find all cpp/c files 42 | 43 | ### Select controller files 44 | CTRL_INC = $(shell find $(SRC_FOLDER) -name $(CONTROLLER).cpp -printf '%h\n') 45 | SOURCES_CPP += $(shell find $(CTRL_INC) -type f -name *.cpp -print) 46 | SOURCES_C += $(shell find $(CTRL_INC) -type f -name *.c -print) 47 | INC_DIRS += $(shell find $(CTRL_INC) -type d) 48 | 49 | ### Select agent files 50 | AGNT_INC = $(shell find $(SRC_FOLDER) -name $(AGENT).cpp -printf '%h\n') 51 | SOURCES_CPP += $(shell find $(AGNT_INC) -name *.cpp -print) 52 | SOURCES_C += $(shell find $(AGNT_INC) -name *.c -print) 53 | INC_DIRS += $(shell find $(AGNT_INC) -type d) 54 | 55 | ### External libraries 56 | # If using pytorch, load the library! 57 | ifeq ($(CONTROLLER),pytorch) 58 | OPT += -L $(TORCH_LIB_HOME)/lib -lc10 -L $(TORCH_LIB_HOME)/lib -ltorch_cpu 59 | INC_DIRS += $(shell find $(TORCH_LIB_HOME) -type d) ## Add torchlib include folder, if present 60 | endif 61 | 62 | # Prepare includes 63 | INC_PARAMS = $(foreach d, $(INC_DIRS), -I$d) # Each include folder must have a -I before it 64 | INC = -I. -I$(SRC_FOLDER) -I$(BUILD_FOLDER) $(INC_PARAMS) # All include paths 65 | 66 | MAKE = $(CC) $(CFLAGS) $(INC) 67 | OBJECTS_CPP=$(SOURCES_CPP:%.cpp=$(BUILD_FOLDER)/%.o) 68 | OBJECTS_C=$(SOURCES_C:%.c=$(BUILD_FOLDER)/%.o) 69 | 70 | # Build the executable 71 | # Using @...; suppresses the output of the arguments 72 | all: $(TARGET) 73 | 74 | $(TARGET): xsd $(OBJECTS_C) $(OBJECTS_CPP) 75 | # Building $(TARGET) 76 | @$(MAKE) $(BUILD_FOLDER)/*.cxx $(OBJECTS_C) $(OBJECTS_CPP) -o $@ $(OPT); 77 | 78 | xsd: 79 | # Generating parameters file 80 | @mkdir -p $(BUILD_FOLDER); 81 | @xsdcxx cxx-tree --output-dir "$(BUILD_FOLDER)" --root-element-all conf/parameters.xsd; 82 | 83 | $(BUILD_FOLDER)/%.o: %.cpp # This rule defines how to go from CPP file to Object file 84 | # Compiling $< 85 | @mkdir -p $(@D) 86 | @$(MAKE) -c $< -o $@ $(OPT); 87 | 88 | $(BUILD_FOLDER)/%.o: %.c # This rule defines how to go from C file to Object file 89 | # Compiling $< 90 | @mkdir -p $(@D) 91 | @gcc -g -Wall -DDEBUG -DINFO -D_GLIBCXX_USE_NANOSLEEP -std=c11 $(INC) -c $< -o $@; 92 | 93 | 94 | clean: 95 | # Cleaning $(TARGET)... 96 | @$(RM) -r $(BUILD_FOLDER); # Remove build folder 97 | @$(RM) -r $(TARGET); # Remove application 98 | -------------------------------------------------------------------------------- /sw/simulation/fitness_functions.h: -------------------------------------------------------------------------------- 1 | #ifndef FITNESS_FUNCTIONS_H 2 | #define FITNESS_FUNCTIONS_H 3 | 4 | #include "omniscient_observer.h" 5 | 6 | /** 7 | * Mean number of neighbors 8 | * 9 | * @return float the mean number of neighbors that each robot has 10 | */ 11 | inline static float mean_number_of_neighbors() 12 | { 13 | float f = 0.; 14 | OmniscientObserver o; 15 | for (size_t ID = 0; ID < s.size(); ID++) { 16 | std::vector closest = o.request_closest_inrange(ID, s[ID]->controller->get_max_sensor_range()); 17 | f += (float)closest.size() / (float)s.size(); 18 | } 19 | return f; 20 | } 21 | 22 | /** 23 | * Mean distance to all 24 | * 25 | * @return float the mean distance between all robots 26 | */ 27 | inline static float mean_dist_to_all() 28 | { 29 | float f = 0.; 30 | OmniscientObserver o; 31 | for (size_t ID = 0; ID < s.size(); ID++) { 32 | std::vector r, b; 33 | o.relative_location(ID, r, b); 34 | float r_mean = accumulate(r.begin(), r.end(), 0.0) / r.size(); 35 | f += (float)r_mean / (float)s.size(); 36 | } 37 | return f; 38 | } 39 | 40 | /** 41 | * Mean distance to neighbors 42 | * 43 | * @return float the mean distance between neighboring robots 44 | */ 45 | inline static float mean_dist_to_neighbors() 46 | { 47 | float f = 0.; 48 | OmniscientObserver o; 49 | for (size_t ID = 0; ID < s.size(); ID++) { 50 | std::vector r, b; 51 | o.relative_location_inrange(ID, s[ID]->controller->get_max_sensor_range(), r, b); 52 | float r_mean = accumulate(r.begin(), r.end(), 0.0) / r.size(); 53 | f += (float)r_mean / (float)s.size(); 54 | } 55 | return f; 56 | } 57 | 58 | /** 59 | * Connectivity 60 | * 61 | * Change f to 0.0 if the graph of the swarm is disconnected, else keeps f 62 | */ 63 | inline static void connectivity_check(float &f) 64 | { 65 | OmniscientObserver o; 66 | if (!(o.connected_graph_range(s[0]->controller->get_max_sensor_range()))) { 67 | f = 0.0; 68 | } 69 | } 70 | 71 | /** 72 | * @brief Measures the number of clusters that are formed 73 | * 74 | * @return uint 75 | */ 76 | inline static uint number_of_clusters() 77 | { 78 | OmniscientObserver o; 79 | Graph g(s.size()); 80 | for (size_t ID = 0; ID < s.size(); ID++) { 81 | std::vector neighbors = o.request_closest_inrange(ID, s[ID]->controller->get_max_sensor_range()); 82 | for (size_t j = 0; j < neighbors.size(); j++) { 83 | g.addEdge(ID, neighbors[j]); 84 | } 85 | } 86 | uint a = g.connectedComponents(); 87 | return a; 88 | } 89 | 90 | /** 91 | * Select a fitness function, or use your own if you want. 92 | * 93 | * @return float fitness 94 | */ 95 | inline static float evaluate_fitness() 96 | { 97 | float f = 1.; // Default 98 | 99 | // Define the fitness function that you would like to use, or write your own 100 | if (!strcmp(param->fitness().c_str(), "mean_number_of_neighbors")) 101 | { f = mean_number_of_neighbors();} 102 | else if (!strcmp(param->fitness().c_str(), "mean_dist_to_neighbors")) 103 | { f = mean_dist_to_neighbors();} 104 | else if (!strcmp(param->fitness().c_str(), "aggregation_clusters")) 105 | { f = 1. / ((float)number_of_clusters() / float(nagents));} 106 | else if (!strcmp(param->fitness().c_str(), "dispersion_clusters")) 107 | { f = ((float)number_of_clusters() / float(nagents));} 108 | else if (!strcmp(param->fitness().c_str(), "aggregation_dist_to_all")) 109 | { f = 1. / mean_dist_to_all();} 110 | else if (!strcmp(param->fitness().c_str(), "dispersion_dist_to_all")) 111 | { f = mean_dist_to_all();} 112 | else if (!strcmp(param->fitness().c_str(), "food")) 113 | { f = (float)environment.nest;} 114 | else if (!strcmp(param->fitness().c_str(), "connected")) 115 | { connectivity_check(f);} 116 | return f; 117 | } 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /sw/math/trigonometry.h: -------------------------------------------------------------------------------- 1 | #ifndef TRIGONOMETRY_H 2 | #define TRIGONOMETRY_H 3 | 4 | #include // qsort 5 | #include 6 | #include 7 | 8 | /** 9 | * Wraps an angle in radians between -PI and +PI, overwrites onto the variable 10 | * 11 | * @param ang Angle value (in radians) 12 | */ 13 | inline static void wrapToPi(float &ang) 14 | { 15 | if (ang > M_PI) { while (ang > M_PI) { ang = ang - 2 * M_PI;} } 16 | else if (ang < -M_PI) { while (ang < -M_PI) { ang = ang + 2 * M_PI;} } 17 | } 18 | 19 | /** 20 | * Wraps an angle in radians between 0 and +2PI, overwrites onto the variable 21 | * 22 | * @param ang Angle value (in radians) 23 | */ 24 | inline static void wrapTo2Pi(float &ang) 25 | { 26 | while (ang > 2 * M_PI) { 27 | ang = ang - 2 * M_PI; 28 | } 29 | while (ang < 0.0) { 30 | ang = ang + 2 * M_PI; 31 | } 32 | } 33 | 34 | /** 35 | * Wraps an angle in radians between -PI and +PI, returns the wrapped value 36 | * 37 | * @param ang Angle value (in radians) 38 | * @return The angle value, wrapped to [-PI,PI] 39 | */ 40 | inline static float wrapToPi_f(float ang) 41 | { 42 | if (ang > M_PI) { 43 | while (ang > M_PI) { 44 | ang = ang - 2 * M_PI; 45 | } 46 | } else if (ang < -M_PI) { 47 | while (ang < -M_PI) { 48 | ang = ang + 2 * M_PI; 49 | } 50 | } 51 | return ang; 52 | } 53 | 54 | /** 55 | * Wraps an angle in radians between 0 and +2PI, returns the wrapped value 56 | * 57 | * @param ang Angle value (in radians) 58 | * @return The angle value, wrapped to [0,2PI] 59 | */ 60 | inline static float wrapTo2Pi_f(float ang) 61 | { 62 | if (ang > 2 * M_PI) { 63 | while (ang > 2 * M_PI) {ang = ang - 2 * M_PI;} 64 | } else if (ang < 0.0) { 65 | while (ang < 0.0) { 66 | ang = ang + 2 * M_PI; 67 | } 68 | } 69 | return ang; 70 | } 71 | 72 | /** 73 | * Converts an angle from radians to degrees 74 | * 75 | * @param ang Angle value in radians 76 | * @return The angle value in degrees 77 | */ 78 | inline static float rad2deg(float rad) { return 180.0 / M_PI * rad; } 79 | 80 | /** 81 | * Converts an angle from degrees to radians 82 | * 83 | * @param ang Angle value in degrees 84 | * @return The angle value in radians 85 | */ 86 | inline static float deg2rad(float deg) { return M_PI / 180.0 * deg; } 87 | 88 | /** 89 | * Convert polar (r,theta) coordinates to cartesian (x,y) coordinates 90 | * 91 | * @param r Value of r in polar coordinates 92 | * @param theta Value of theta in polar coordinates (in radians) 93 | * @param x Value of x in cartesian coordinates (East) 94 | * @param y Value of y in cartesian coordinates (North) 95 | */ 96 | inline static void polar2cart(const float &r, const float &theta, float &x, float &y) 97 | { 98 | x = r * cos(wrapToPi_f(theta)); 99 | y = r * sin(wrapToPi_f(theta)); 100 | } 101 | 102 | /** 103 | * Convert cartesian (x,y) coordinates to polar (r,theta) coordinates 104 | * 105 | * @param x Value of x in cartesian coordinates (East) 106 | * @param y Value of y in cartesian coordinates (North) 107 | * @param r Value of r in polar coordinates 108 | * @param theta Value of theta in polar coordinates 109 | */ 110 | inline static void cart2polar(const float &x, const float &y, float &r, float &theta) 111 | { 112 | r = sqrt(pow(x, 2) + pow(y, 2)); 113 | theta = atan2(y, x); 114 | } 115 | 116 | /** 117 | * Rotates a vector (x,y) by a certain angle theta 118 | * 119 | * @param x Value of x in cartesian coordinates (East) 120 | * @param y Value of y in cartesian coordinates (North) 121 | * @param theta Angle of rotation 122 | * @param xr Rotated x 123 | * @param yr Rotated y 124 | */ 125 | inline static void rotate_xy(const float &x, const float &y, const float &theta, float &xr, float &yr) 126 | { 127 | xr = x * cos(theta) - y * sin(theta); 128 | yr = x * sin(theta) + y * cos(theta); 129 | } 130 | 131 | #endif /*TRIGONOMETRY_H*/ 132 | -------------------------------------------------------------------------------- /sw/simulation/controllers/pattern_formation/lattice_basic.cpp: -------------------------------------------------------------------------------- 1 | #include "lattice_basic.h" 2 | #include "agent.h" 3 | #include "main.h" 4 | #include "randomgenerator.h" 5 | #include "trigonometry.h" 6 | 7 | using namespace std; 8 | #define SENSORS 8 9 | #define SENSOR_MAX_RANGE 1.8 10 | 11 | lattice_basic::lattice_basic(): t(SENSORS, SENSOR_MAX_RANGE) 12 | { 13 | sensor_range = SENSOR_MAX_RANGE; 14 | _v_adj = 1.0; 15 | d_safe = 0.9; 16 | }; 17 | 18 | float lattice_basic::f_attraction(const float &u, const float &b_eq) 19 | { 20 | float w; 21 | float ddes2 = sqrt(pow(_ddes_x, 2.0) + pow(_ddes_y, 2.0)); 22 | float tol = 0.1; 23 | 24 | // TODO: move general attraction equation to controller 25 | if (abs(b_eq - atan(_ddes_y / _ddes_x)) < tol || // +- 45 deg 26 | abs(b_eq - M_PI / 2.0 - atan(_ddes_x / _ddes_y)) < tol) { // +- 135 deg 27 | w = log((ddes2 / _kr - 1) / exp(-_ka * ddes2)) / _ka; 28 | } else if (abs(b_eq) - deg2rad(180) < tol) { // 0,180 29 | w = log((_ddes_y / _kr - 1) / exp(-_ka * _ddes_y)) / _ka; 30 | } else if (abs(b_eq - deg2rad(90)) < tol) { 31 | w = log((_ddes_x / _kr - 1) / exp(-_ka * _ddes_x)) / _ka; 32 | } 33 | 34 | return 1 / (1 + exp(-_ka * (u - w))); 35 | } 36 | 37 | // TODO -- move general sum to controller 38 | float lattice_basic::get_attraction_velocity(const float &u, const float &b_eq) 39 | { 40 | return f_attraction(u, b_eq) + f_repulsion(u); 41 | } 42 | 43 | void lattice_basic::attractionmotion(const float &v_r, const float &v_b, float &v_x, float &v_y) 44 | { 45 | v_x += v_r * cos(v_b); 46 | v_y += v_r * sin(v_b); 47 | } 48 | 49 | void lattice_basic::latticemotion(const float &v_r, const float &v_adj, const float &v_b, const float &bdes, 50 | float &v_x, float &v_y) 51 | { 52 | attractionmotion(v_r + v_adj, v_b, v_x, v_y); 53 | v_x += -v_adj * cos(bdes * 2 - v_b); 54 | v_y += -v_adj * sin(bdes * 2 - v_b); 55 | } 56 | 57 | void lattice_basic::actionmotion(const int &selected_action, float &v_x, float &v_y) 58 | { 59 | float actionspace_y[8] = {0, sqrt(_ddes_y) / 3, _ddes_y, sqrt(_ddes_y) / 3, 0, -sqrt(_ddes_y) / 3, -_ddes_y, -sqrt(_ddes_y) / 3}; 60 | float actionspace_x[8] = {_ddes_x, sqrt(_ddes_x) / 3, 0, -sqrt(_ddes_x) / 3, -_ddes_x, -sqrt(_ddes_x) / 3, 0, sqrt(_ddes_x) / 3}; 61 | v_x = _v_adj * actionspace_x[selected_action]; 62 | v_y = _v_adj * actionspace_y[selected_action]; 63 | } 64 | 65 | bool lattice_basic::check_motion(const vector &state_ID) 66 | { 67 | bool canImove = true; 68 | for (uint16_t i = 0; i < state_ID.size(); i++) { 69 | if (o.see_if_moving(state_ID[i])) { 70 | // Somebody nearby is already moving 71 | return canImove; 72 | } 73 | } 74 | return canImove; 75 | } 76 | 77 | void lattice_basic::get_lattice_motion(const int &ID, const int &state_ID, float &v_x, float &v_y) 78 | { 79 | float beta, b_eq, v_r; 80 | beta = wrapToPi_f(o.request_bearing(ID, state_ID)); 81 | b_eq = t.get_preferred_bearing(beta_des, beta); 82 | v_r = get_attraction_velocity(o.request_distance(ID, state_ID), b_eq); 83 | latticemotion(v_r, _v_adj, beta, b_eq, v_x, v_y); 84 | // Uncomment to add rotation (untested) 85 | // float b, r, px, py; 86 | // b = wrapToPi_f(o.request_bearing(ID, state_ID)); 87 | // r = o.request_distance(ID, state_ID); 88 | // polar2cart(r, b, px, py); 89 | // v_x += 1 * (cos(b) - sin(b)); 90 | // v_y += 1 * (sin(b) + cos(b)); 91 | } 92 | 93 | void lattice_basic::get_lattice_motion_all(const int &ID, const vector &state_ID, 94 | const vector &closest, float &v_x, float &v_y) 95 | { 96 | if (!state_ID.empty()) { 97 | if (o.request_distance(ID, closest[0]) > d_safe) { // If all are further than the safety distance 98 | for (size_t i = 0; i < state_ID.size(); i++) { 99 | get_lattice_motion(ID, state_ID[i], v_x, v_y); 100 | } 101 | v_x = v_x / (float)state_ID.size(); 102 | v_y = v_y / (float)state_ID.size(); 103 | } else { // Focus on the closest one only 104 | get_lattice_motion(ID, state_ID[0], v_x, v_y); 105 | } 106 | } 107 | } 108 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/composites.h: -------------------------------------------------------------------------------- 1 | #ifndef COMPOSITES_H 2 | #define COMPOSITES_H 3 | 4 | #include "node.h" 5 | 6 | namespace BT 7 | { 8 | typedef std::vector nodes; 9 | 10 | /** 11 | * Composite behaviors 12 | * 13 | */ 14 | class composite : public node 15 | { 16 | public: 17 | /** 18 | * @brief 19 | * 20 | */ 21 | void reset(); 22 | 23 | /** 24 | * @brief 25 | * 26 | * @param child 27 | * @param position 28 | */ 29 | void addChild(node *child, int position = -1); 30 | 31 | /** 32 | * @brief Set the Children object 33 | * 34 | * @param children 35 | * @return nodes 36 | */ 37 | nodes setChildren(nodes children); 38 | 39 | /** 40 | * @brief Get the Child object 41 | * 42 | * @param k_child 43 | * @return node* 44 | */ 45 | node *getChild(size_t k_child); 46 | 47 | /** 48 | * @brief Get the Children object 49 | * 50 | * @return nodes 51 | */ 52 | nodes getChildren(); 53 | 54 | /** 55 | * @brief 56 | * 57 | * @return size_t 58 | */ 59 | size_t kChildren(); 60 | 61 | /** 62 | * @brief 63 | * 64 | * @param task 65 | * @param k_child 66 | * @param isNEW 67 | * @return node* 68 | */ 69 | node *replaceChild(node *task, size_t k_child, size_t isNEW = 0); 70 | 71 | /** 72 | * @brief 73 | * 74 | * @param task 75 | * @param parallel_found 76 | * @return true 77 | * @return false 78 | */ 79 | bool canFit(node *task, size_t parallel_found); 80 | 81 | /** 82 | * @brief 83 | * 84 | * @param k_child 85 | */ 86 | void deleteChild(size_t k_child); 87 | 88 | /** 89 | * @brief Destroy the composite object 90 | * 91 | */ 92 | virtual ~composite() 93 | { 94 | for (m_current = m_children.begin(); m_current != m_children.end(); m_current++) { 95 | delete *m_current; 96 | } 97 | m_children.clear(); 98 | } 99 | 100 | protected: 101 | /** 102 | * @brief Construct a new composite object 103 | * 104 | * @param Name 105 | * @param Type 106 | * @param Function 107 | */ 108 | composite(std::string Name, std::string Type, std::string Function) : 109 | node(Name, Type, Function) 110 | { 111 | } 112 | 113 | /** 114 | * @brief 115 | * 116 | */ 117 | virtual void onInitialise() { m_current = m_children.begin(); } 118 | 119 | nodes m_children; 120 | nodes::iterator m_current; 121 | }; 122 | 123 | /** 124 | * @brief Sequence behaviors 125 | * 126 | */ 127 | class sequence : public composite 128 | { 129 | protected: 130 | /** 131 | * @brief 132 | * 133 | * @param BLKB 134 | * @return BT_Status 135 | */ 136 | BT_Status update(blackboard *BLKB); 137 | public: 138 | /** 139 | * @brief Construct a new sequence object 140 | * 141 | */ 142 | sequence(): 143 | composite("sequence", "composite", "sequence") 144 | { 145 | } 146 | }; 147 | 148 | /** 149 | * @brief Selector behaviors 150 | * 151 | */ 152 | class selector : public composite 153 | { 154 | protected: 155 | /** 156 | * @brief 157 | * 158 | * @param BLKB 159 | * @return BT_Status 160 | */ 161 | BT_Status update(blackboard *BLKB); 162 | public: 163 | selector(): 164 | composite("selector", "composite", "selector") 165 | { 166 | } 167 | }; 168 | 169 | /** 170 | * @brief Parallel behaviors 171 | * 172 | */ 173 | class parallel : public composite 174 | { 175 | protected: 176 | BT_Status update(blackboard *BLKB); 177 | public: 178 | parallel(): 179 | composite("parallel", "composite", "parallel") 180 | { 181 | } 182 | }; 183 | 184 | /** 185 | * @brief Get the Composite object 186 | * 187 | * @param composite 188 | * @return composite* 189 | */ 190 | composite *getComposite(std::string composite); 191 | //composite *getComposite(std::string composite, std::vector* values); 192 | 193 | /** 194 | * @brief Get the Composite object 195 | * 196 | * @param func 197 | * @return composite* 198 | */ 199 | composite *getComposite(size_t func); 200 | 201 | /** 202 | * @brief Get the Composite object 203 | * 204 | * @return composite* 205 | */ 206 | composite *getComposite(); 207 | 208 | /** 209 | * @brief 210 | * 211 | * @param task 212 | * @param tab 213 | */ 214 | void showTree(composite *task, size_t tab); 215 | } 216 | #endif // COMPOSITES_H 217 | -------------------------------------------------------------------------------- /sw/simulation/sensors/template_calculator.cpp: -------------------------------------------------------------------------------- 1 | #include "template_calculator.h" 2 | #include "main.h" 3 | #include "trigonometry.h" 4 | #include // std::find 5 | 6 | using namespace std; 7 | 8 | Template_Calculator::Template_Calculator(uint spacing, float r) 9 | { 10 | sp = spacing; 11 | range = r; 12 | // Angles to check for neighboring links 13 | for (size_t i = 0; i <= sp; i++) { 14 | blink.push_back(2. * M_PI * (float)i / (float)sp); 15 | } 16 | }; 17 | 18 | void Template_Calculator::set_state_action_matrix(string filename) 19 | { 20 | state_action_matrix.clear(); 21 | ifstream state_action_matrix_file(filename); 22 | 23 | if (state_action_matrix_file.is_open()) { 24 | // Collect the data inside the a stream, do this line by line 25 | while (!state_action_matrix_file.eof()) { 26 | string line; 27 | getline(state_action_matrix_file, line); 28 | stringstream stream_line(line); 29 | int buff[10] = {}; 30 | int columns = 0; 31 | int state_index; 32 | bool index_checked = false; 33 | while (!stream_line.eof()) { 34 | if (!index_checked) { 35 | stream_line >> state_index; 36 | index_checked = true; 37 | } else { 38 | stream_line >> buff[columns]; 39 | columns++; 40 | } 41 | } 42 | if (columns > 0) { 43 | vector actions_index(begin(buff), begin(buff) + columns); 44 | state_action_matrix.insert(pair>(state_index, actions_index)); 45 | } 46 | } 47 | state_action_matrix_file.close(); 48 | } else { 49 | terminalinfo::error_msg("Unable to open state action matrix file."); 50 | } 51 | } 52 | 53 | // TODO: Make smarter 54 | bool Template_Calculator::fill_template(vector &q, const float &b_i, const float &u, const float &dmax, 55 | const float &angle_err) 56 | { 57 | // Determine link (cycle through all options) 58 | if (u < dmax) { // If in range of sensor 59 | for (int j = 0; j < (int)blink.size(); j++) { // For all angle options 60 | if (abs(b_i - blink[j]) < angle_err 61 | && !q[j]) { // If in the right angle and not already taken by another agent 62 | if (j == (int)blink.size() - 1) { // last element is back to 0 63 | j = 0; 64 | } 65 | q[j] = true; 66 | return true; 67 | } 68 | } 69 | } 70 | return false; 71 | } 72 | 73 | // TODO: Make smarter 74 | float Template_Calculator::get_preferred_bearing(const vector &bdes, const float v_b) 75 | { 76 | // Define in bv all equilibrium angles at which the agents can organize themselves 77 | vector bv; 78 | for (int i = 0; i < 5; i++) { 79 | for (int j = 0; j < (int)bdes.size(); j++) { 80 | bv.push_back(bdes[j]); 81 | } 82 | } 83 | 84 | // Find what the desired angle is in bdes 85 | for (int i = 0; i < (int)bv.size(); i++) { 86 | if (i < (int)bdes.size() * 1) { 87 | bv[i] = abs(bv[i] - 2 * M_PI - v_b); 88 | } else if (i < (int)bdes.size() * 2) { 89 | bv[i] = abs(bv[i] - M_PI - v_b); 90 | } else if (i < (int)bdes.size() * 3) { 91 | bv[i] = abs(bv[i] - v_b); 92 | } else if (i < (int)bdes.size() * 4) { 93 | bv[i] = abs(bv[i] + M_PI - v_b); 94 | } else if (i < (int)bdes.size() * 5) { 95 | bv[i] = abs(bv[i] + 2 * M_PI - v_b); 96 | } 97 | } 98 | 99 | int minindex = 0; 100 | for (int i = 1; i < (int)bv.size(); i++) { 101 | if (bv[i] < bv[minindex]) { 102 | minindex = i; 103 | } 104 | } 105 | 106 | // Reduce the index for the angle of interest from bdes 107 | while (minindex >= (int)bdes.size()) { 108 | minindex -= (int)bdes.size(); 109 | } 110 | 111 | // Returned the desired equilibrium bearing 112 | return bdes[minindex]; 113 | } 114 | 115 | void Template_Calculator::assess_situation(uint16_t ID, vector &q, vector &q_ID) 116 | { 117 | q.clear(); 118 | q.assign(sp, false); 119 | q_ID.clear(); 120 | 121 | // Fill the template with respect to the agent in question 122 | vector closest = o.request_closest(ID); 123 | for (uint16_t i = 0; i < s.size() - 1; i++) { 124 | if (fill_template(q, // Vector to fill 125 | wrapTo2Pi_f(o.request_bearing(ID, closest[i])), // Bearing 126 | o.request_distance(ID, closest[i]), // Distance 127 | range, M_PI / (float)sp)) { // Sensor range, bearing precision 128 | // Use higher values of bearing sensor to handle higher noise values (even if there is overlap) 129 | q_ID.push_back(closest[i]); // Log ID (for simulation purposes only, depending on assumptions) 130 | } 131 | } 132 | } 133 | -------------------------------------------------------------------------------- /sw/simulation/agent_thread.h: -------------------------------------------------------------------------------- 1 | #ifndef AGENT_THREAD_H 2 | #define AGENT_THREAD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "settings.h" 13 | #include "randomgenerator.h" 14 | #include "environment.h" 15 | #include "terminalinfo.h" 16 | #include "auxiliary.h" 17 | 18 | // Include relevant h file for the agent. 19 | // This path is defined at build time depending on the chosen agent. 20 | // No need to change this. 21 | #include AGENT_INCLUDE 22 | 23 | /** 24 | * Update the agent simulation 25 | * @param ID The ID of the agent/robot 26 | */ 27 | void run_agent_simulation_step(const int &ID) 28 | { 29 | while (program_running) { 30 | 31 | bool ready = (s.size() == nagents || simtime_seconds > 0.); 32 | if (ready) { 33 | mtx.lock_shared(); 34 | std::vector s_0 = s.at(ID)->state; 35 | std::vector s_n = s.at(ID)->state_update(s_0); // State update 36 | mtx.unlock_shared(); 37 | 38 | /****** Wall physics engine ********/ 39 | // The dynamics are updated unless the robot crashes into a wall, at which point the velocity and acceleration are set to 0. 40 | // The crash into a wall is detected by checking whether the robot is near a wall and the velocity vector intersects with the wall. 41 | 42 | // Check if hitting a wall 43 | std::vector test = s_n; // Store a copy of the state update 44 | float r_temp, ang_temp, vx_temp, vy_temp; 45 | 46 | // Get direction of velocity vector 47 | cart2polar(s_n[2], s_n[3], r_temp, ang_temp); 48 | 49 | // Detect a potential crash by projecting the position forward based on the velocity, based on a simple test of whether the collision could happen within one second, provided the current velocity is kept. 50 | // Here we just project the velocity. 51 | polar2cart(r_temp, ang_temp, vx_temp, vy_temp); 52 | test[0] += vx_temp; 53 | test[1] += vy_temp; 54 | 55 | // Check if hitting the wall. 56 | // Update or kill the dynamics in case of collision. 57 | // The environment.sensor function checks whether the vector s0->test intersects with any walls. 58 | if (!environment.sensor(ID, s_0, test, ang_temp)) { 59 | // No wall collision --> Update the dynamics 60 | mtx.lock(); // Sync 61 | s.at(ID)->state = s_n; // Update 62 | mtx.unlock(); 63 | } 64 | 65 | else { // Wall! --> Kill the dynamics! 66 | mtx.lock(); // Sync 67 | s.at(ID)->state[2] = 0.0; // v_x = 0 68 | s.at(ID)->state[3] = 0.0; // v_y = 0 69 | s.at(ID)->state[4] = 0.0; // a_x = 0 70 | s.at(ID)->state[5] = 0.0; // a_y = 0 71 | s.at(ID)->controller->moving = false; // Not moving 72 | mtx.unlock(); 73 | } 74 | 75 | // Update the global clock. 76 | // The clock of the 0th robot is used as the reference for the global clock. 77 | if (ID == 0) { 78 | simtime_seconds += 1. / param->simulation_updatefreq(); 79 | environment.loop(); 80 | } 81 | 82 | 83 | // Run the local clock 84 | // If param->simulation_realtimefactor()=0, which can be set in conf/parameters.xml, the thread will not sleep and just run at full speed. 85 | if (param->simulation_realtimefactor() > 0) { 86 | int t_wait = (int)1e6 / (param->simulation_updatefreq() * param->simulation_realtimefactor()); 87 | std::this_thread::sleep_for(std::chrono::microseconds(t_wait)); 88 | } 89 | } 90 | } 91 | } 92 | 93 | /** 94 | * Generates new agent + simulation thread at given position x0 y0 95 | * 96 | * @param ID The ID of the agent/robot 97 | * @param x Initial position of the agent in x 98 | * @param y Initial position of the agent in y 99 | */ 100 | void create_new_agent(const int &ID, const std::vector &states) 101 | { 102 | // Initiate a new agent. The define "AGENT" is defined at build time. 103 | // No need to change this part of the code if you want to use a different agent, just compile swarmulator with: 104 | // >> make AGENT=myawesomeagent 105 | // By default, AGENT=particle 106 | // See wiki for a detailed explanation of how to create your own agent. 107 | mtx.lock(); 108 | s.push_back(new AGENT(ID, states, 1.0 / param->simulation_updatefreq())); 109 | mtx.unlock(); 110 | 111 | // Info message 112 | std::stringstream ss; 113 | ss << "Robot " << ID << " initiated"; 114 | terminalinfo::info_msg(ss.str()); 115 | 116 | // Initiate the thread that controls the agent 117 | std::thread agent(run_agent_simulation_step, ID); 118 | 119 | // Detach thread so that it runs independently 120 | agent.detach(); 121 | } 122 | #endif /*AGENT_THREAD_H*/ 123 | -------------------------------------------------------------------------------- /sw/simulation/environment.cpp: -------------------------------------------------------------------------------- 1 | #include "environment.h" 2 | #include "main.h" 3 | #include "settings.h" 4 | #include "auxiliary.h" 5 | #include "draw.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | 15 | Environment::Environment(void) 16 | { 17 | define_walls(); 18 | if (!strcmp(param->fitness().c_str(), "food")) { 19 | mtx_env.lock(); 20 | environment.define_food(100); 21 | environment.define_beacon(0., 0.); 22 | environment.nest = 8; 23 | mtx_env.unlock(); 24 | } 25 | } 26 | 27 | void Environment::define_walls(void) 28 | { 29 | string s = param->environment(); 30 | if (!strcmp(s.c_str(), "random")) { 31 | stringstream ss("python3 scripts/python/tools/dungeon_generator.py && mv rooms.txt conf/environments/random.txt"); 32 | system(ss.str().c_str()); 33 | terminalinfo::info_msg("Generating random environment"); 34 | } 35 | string filename = "conf/environments/" + param->environment() + ".txt"; 36 | walls = read_matrix(filename); 37 | } 38 | 39 | void Environment::define_food(uint64_t n) 40 | { 41 | float lim = limits(); 42 | for (size_t i = 0; i < n; i++) { 43 | food.push_back(std::vector()); 44 | food[i].push_back(rg.uniform_float(-lim, lim)); 45 | food[i].push_back(rg.uniform_float(-lim, lim)); 46 | } 47 | } 48 | 49 | void Environment::define_beacon(float x, float y) 50 | { 51 | beacon.push_back(x); 52 | beacon.push_back(y); 53 | } 54 | 55 | // TODO: Temporary function for initialization, but the initalization should change eventually 56 | std::vector Environment::start(void) 57 | { 58 | std::vector s(2); 59 | s[0] = walls[0][0] + 1.0; 60 | s[1] = walls[0][1] - 1.0; 61 | return s; 62 | } 63 | 64 | // TODO: Temporary function for initialization, but the initalization should change eventually 65 | float Environment::limits(void) 66 | { 67 | float max = 0; 68 | for (size_t i = 0; i < walls.size(); i++) { 69 | float v = *max_element(walls[i].begin(), walls[i].end()); // c++11 70 | if (max < v) { 71 | max = v; 72 | } 73 | } 74 | return max * 0.95; // 0.95 for margin 75 | } 76 | 77 | void Environment::add_wall(float x0, float y0, float x1, float y1) 78 | { 79 | mtx.lock(); 80 | walls.push_back(std::vector()); 81 | walls[walls.size() - 1].push_back(x0); 82 | walls[walls.size() - 1].push_back(y0); 83 | walls[walls.size() - 1].push_back(x1); 84 | walls[walls.size() - 1].push_back(y1); 85 | mtx.unlock(); 86 | } 87 | 88 | bool Environment::sensor(const uint16_t ID, std::vector s_n, std::vector s, float &angle) 89 | { 90 | Point p1, q1, p2, q2; 91 | p1.y = s[0]; // Flip axis 92 | p1.x = s[1]; 93 | q1.y = s_n[0]; 94 | q1.x = s_n[1]; 95 | for (size_t i = 0; i < walls.size(); i++) { 96 | p2.x = walls[i][0]; 97 | p2.y = walls[i][1]; 98 | q2.x = walls[i][2]; 99 | q2.y = walls[i][3]; 100 | if (doIntersect(p1, q1, p2, q2)) { 101 | angle = atan2(p2.y - q2.y, p2.x - q2.x); 102 | return true; 103 | } 104 | } 105 | return false; 106 | } 107 | 108 | bool Environment::valid(const uint16_t ID, std::vector s_n, std::vector s) 109 | { 110 | Point p1, q1, p2, q2; 111 | p1.y = s[0]; // Flip axis 112 | p1.x = s[1]; 113 | q1.y = s_n[0]; 114 | q1.x = s_n[1]; 115 | uint v = 0; 116 | for (size_t i = 0; i < walls.size(); i++) { 117 | p2.x = walls[i][0]; 118 | p2.y = walls[i][1]; 119 | q2.x = walls[i][2]; 120 | q2.y = walls[i][3]; 121 | if (doIntersect(p1, q1, p2, q2)) { 122 | v += 1; 123 | } 124 | } 125 | // Hits an even number of walls, incl 0 126 | if (v % 2 == 0) { 127 | return true; 128 | } 129 | 130 | return false; 131 | } 132 | 133 | void Environment::animate(void) 134 | { 135 | draw d; 136 | for (size_t i = 0; i < walls.size(); i++) { 137 | d.segment(walls[i][0], walls[i][1], walls[i][2], walls[i][3]); 138 | } 139 | 140 | for (size_t i = 0; i < food.size(); i++) { 141 | d.food(food[i][0], food[i][1]); 142 | } 143 | } 144 | 145 | 146 | void Environment::grab_food(uint64_t food_ID) 147 | { 148 | float lim = limits(); 149 | mtx_env.lock(); 150 | // uncomment one of the two line below 151 | // food.erase(food.begin() + food_ID); // Use this to grab without replacement 152 | food[food_ID] = {rg.uniform_float(-lim, lim), rg.uniform_float(-lim, lim)}; // Use this to grab with replacement 153 | mtx_env.unlock(); 154 | } 155 | 156 | void Environment::drop_food() 157 | { 158 | mtx_env.lock(); 159 | nest += 1.; 160 | mtx_env.unlock(); 161 | } 162 | 163 | void Environment::eat_food(float amount) 164 | { 165 | mtx_env.lock(); 166 | if (nest > amount) { nest -= amount; } 167 | mtx_env.unlock(); 168 | } 169 | 170 | void Environment::loop() 171 | { 172 | float rate = (0.001 / param->simulation_updatefreq()) * s.size(); 173 | eat_food(rate); 174 | } 175 | -------------------------------------------------------------------------------- /sw/simulation/controller.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_H 2 | #define CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "omniscient_observer.h" 11 | 12 | /** 13 | * This is a parent class for a controller. 14 | * All controllers are children functions that must contain an implementation of get_velocity_command, which is declared virtual here. 15 | */ 16 | class Controller 17 | { 18 | protected: 19 | OmniscientObserver o; 20 | random_generator rg; 21 | 22 | public: 23 | /** 24 | * Constructor 25 | */ 26 | Controller(); 27 | 28 | /** 29 | * Destructor 30 | */ 31 | ~Controller() {}; 32 | float _ddes_x; // Desired equilibrium distance_x 33 | float _ddes_y; // Desired equilibrium distance_y 34 | float _kr; // Repulsion gain 35 | float _ka; // Attraction gain 36 | bool saturation; // Define whether the controls are saturated 37 | float saturation_limits; // Define the saturation of the controls 38 | bool moving; // Internal state of whether the robot is actively moving or not 39 | bool happy; 40 | float sensor_range_max; 41 | 42 | /** 43 | * Set the speed saturation to true and set the saturation limits. 44 | * If this is not used then the saturation won't take place. 45 | * 46 | * @param saturation_limits The limit of the saturation (velocity) 47 | */ 48 | void set_saturation(const float &saturation_limits); 49 | 50 | /** 51 | * Function to saturate the commands 52 | * 53 | * @param f Value to be saturated 54 | */ 55 | void saturate(float &f); 56 | 57 | /** 58 | * Attraction function 59 | * This is defined here since it is a basic behavior for collision avoidance 60 | * which many higher level controllers may want to use, although it does not have to be. 61 | * @param u Distance to neighbor 62 | * @return float Attraction speed 63 | */ 64 | float f_attraction(float u); 65 | 66 | /** 67 | * Attraction function 68 | * This is defined here since it is a basic behavior for collision avoidance. 69 | * which many higher level controllers may want to use, although it does not have to be. 70 | * @param u Distance to neighbor 71 | * @return float Attraction speed 72 | */ 73 | float f_attraction_equilibrium(float u, float eq_distance); 74 | 75 | /** 76 | * Repulsion function. 77 | * This is defined here since it is a basic behavior for collision avoidance 78 | * which many higher level controllers may want to use, although it does not have to be. 79 | * 80 | * @param u Distance to other robot 81 | * @return Repulsion velocity component 82 | */ 83 | float f_repulsion(float u); 84 | 85 | /** 86 | * Implementation of lattice behavior. 87 | * 88 | * @param ID 89 | * @param state_ID 90 | * @param v_x 91 | * @param v_y 92 | */ 93 | void get_lattice_motion(const int &ID, const int &state_ID, float &v_x, float &v_y); 94 | 95 | /** 96 | * @brief Get the lattice motion all object 97 | * 98 | * @param ID 99 | * @param v_x 100 | * @param v_y 101 | * @param rangesensor maximum range of the sensor 102 | */ 103 | void get_lattice_motion_range(const int &ID, float &v_x, float &v_y, float rangesensor); 104 | 105 | /** 106 | * @brief Get the lattice motion all object 107 | * 108 | * @param ID 109 | * @param v_x 110 | * @param v_y 111 | * @param k (maximum) number of nearest neighbors 112 | */ 113 | void get_lattice_motion_k_nearest(const int &ID, float &v_x, float &v_y, uint8_t k); 114 | 115 | /** 116 | * @brief Get the attraction velocity object 117 | * 118 | * @param u 119 | * @return float 120 | */ 121 | float get_attraction_velocity(float u); 122 | 123 | /** 124 | * Virtual function to be implemented by child class that defines the controller 125 | * The outputs are the desired v_x and v_y velocities 126 | * 127 | * @param ID The ID of the robot to control 128 | * @param v_x The desired velocity in v_x (to be set in this function) 129 | * @param v_y The desired velocity in v_y (to be set in this function) 130 | */ 131 | virtual void get_velocity_command(const uint16_t ID, float &v_x, float &v_y) = 0; 132 | 133 | /** 134 | * General wall avoidance function to use within get_velocity_command() to instigate a wall avoidance maneuver 135 | * 136 | * @param ID The ID of the robot to control 137 | * @param v_x The desired velocity in v_x (amended in this function) 138 | * @param v_y The desired velocity in v_y (amended in this function) 139 | */ 140 | bool wall_avoidance_bounce(const uint16_t ID, float &v_x, float &v_y, float rangesensor); 141 | bool wall_avoidance_turn(const uint16_t ID, float &v_x, float &v_y, float rangesensor); 142 | virtual void animation(const uint16_t ID) = 0; 143 | 144 | float get_max_sensor_range(); 145 | void set_max_sensor_range(float r); 146 | }; 147 | 148 | 149 | #endif /*CONTROLLER_H*/ 150 | -------------------------------------------------------------------------------- /sw/simulation/controller.cpp: -------------------------------------------------------------------------------- 1 | #include "controller.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "main.h" 9 | #include "randomgenerator.h" 10 | #include "trigonometry.h" 11 | #include "auxiliary.h" 12 | 13 | Controller::Controller() 14 | { 15 | _ddes_x = 1.0; // Desired distance at North 16 | _ddes_y = 1.0; // Desired distance at East 17 | _kr = 1.0; // Repulsion gain 18 | _ka = 0.1; // Attraction gain 19 | saturation = false; // Controller saturation 20 | }; 21 | 22 | void Controller::saturate(float &f) 23 | { 24 | if (saturation) { 25 | keepbounded(f, -saturation_limits, saturation_limits); 26 | } 27 | } 28 | 29 | float Controller::f_attraction(float u) 30 | { 31 | return _ka * u; 32 | } 33 | 34 | float Controller::f_attraction_equilibrium(float u, float eq_distance) 35 | { 36 | // Sigmoid function -- long-range 37 | float w = log(abs(eq_distance / _kr - 1.0) / exp(-_ka * eq_distance)) / _ka; 38 | return 1 / (1 + exp(-_ka * (u - w))); 39 | } 40 | 41 | 42 | void Controller::get_lattice_motion(const int &ID, const int &state_ID, float &v_x, float &v_y) 43 | { 44 | float v_b, v_r; 45 | v_b = wrapToPi_f(o.request_bearing(ID, state_ID)); 46 | v_r = get_attraction_velocity(o.request_distance(ID, state_ID)); 47 | v_x += v_r * cos(v_b); 48 | v_y += v_r * sin(v_b); 49 | } 50 | 51 | void Controller::get_lattice_motion_range(const int &ID, float &v_x, float &v_y, float rangemax) 52 | { 53 | std::vector closest = o.request_closest(ID); 54 | std::vector q_ID; 55 | q_ID.clear(); 56 | for (uint16_t i = 0; i < s.size() - 1; i++) { 57 | if (o.request_distance(ID, closest[i]) < rangemax) { 58 | q_ID.push_back(closest[i]); // Log ID (for simulation purposes only, depending on assumptions) 59 | } 60 | } 61 | if (!q_ID.empty()) { 62 | for (size_t i = 0; i < q_ID.size(); i++) { 63 | get_lattice_motion(ID, q_ID[i], v_x, v_y); 64 | } 65 | v_x += v_x / (float)q_ID.size(); 66 | v_y += v_y / (float)q_ID.size(); 67 | } 68 | } 69 | 70 | void Controller::get_lattice_motion_k_nearest(const int &ID, float &v_x, float &v_y, uint8_t k) 71 | { 72 | std::vector closest = o.request_closest(ID); 73 | if (!closest.empty()) { 74 | uint mm = std::min(int(closest.size()), int(k)); 75 | for (size_t i = 0; i < mm; i++) { 76 | get_lattice_motion(ID, closest[i], v_x, v_y); 77 | } 78 | v_x += v_x / (float)k; 79 | v_y += v_y / (float)k; 80 | } 81 | } 82 | 83 | 84 | float Controller::get_attraction_velocity(float u) 85 | { 86 | return f_attraction_equilibrium(u, 1.5) + f_repulsion(u); 87 | } 88 | 89 | float Controller::f_repulsion(float u) 90 | { 91 | return -_kr / u; 92 | } 93 | 94 | void Controller::set_saturation(const float &lim) 95 | { 96 | saturation = true; 97 | saturation_limits = lim; 98 | } 99 | 100 | bool Controller::wall_avoidance_bounce(const uint16_t ID, float &v_x, float &v_y, float rangesensor) 101 | { 102 | // Predict what the command wants and see if it will hit a wall, then fix it. 103 | std::vector sn = s[ID]->state; 104 | float r_temp, ang_temp, vx_temp, vy_temp; 105 | cart2polar(v_x, v_y, r_temp, ang_temp); // direction of velocity 106 | polar2cart(rangesensor, ang_temp, vx_temp, vy_temp); // use rangesensor to sense walls 107 | sn[0] += vx_temp; 108 | sn[1] += vy_temp; 109 | float slope; 110 | bool test = environment.sensor(ID, sn, s[ID]->state, slope); 111 | if (test) { 112 | float v, ang; 113 | cart2polar(v_x, v_y, v, ang); 114 | ang = rg.uniform_float(0, 2 * M_PI); 115 | polar2cart(v, ang, v_x, v_y); 116 | moving = false; 117 | return true; 118 | } 119 | return false; 120 | } 121 | 122 | bool Controller::wall_avoidance_turn(const uint16_t ID, float &v, float &dpsitheta, float rangesensor) 123 | { 124 | // Predict what the command wants and see if it will hit a wall, then fix it. 125 | std::vector sn = s[ID]->state; 126 | float r_temp, ang_temp, vx_temp, vy_temp, vx_global, vy_global, slope; 127 | rotate_xy(0.5, 0.5, sn[6], vx_global, vy_global); 128 | cart2polar(vx_global, vy_global, r_temp, ang_temp); // direction of velocity 129 | polar2cart(rangesensor, ang_temp, vx_temp, vy_temp); // use rangesensor to sense walls 130 | sn[0] += vx_temp; 131 | sn[1] += vy_temp; 132 | bool test1 = environment.sensor(ID, sn, s[ID]->state, slope); 133 | 134 | sn = s[ID]->state; 135 | rotate_xy(0.5, -0.5, sn[6], vx_global, vy_global); 136 | cart2polar(vx_global, vy_global, r_temp, ang_temp); // direction of velocity 137 | polar2cart(rangesensor, ang_temp, vx_temp, vy_temp); // use rangesensor to sense walls 138 | sn[0] += vx_temp; 139 | sn[1] += vy_temp; 140 | 141 | bool test2 = environment.sensor(ID, sn, s[ID]->state, slope); 142 | if (test1 || test2) { 143 | v = 0.; 144 | dpsitheta = 0.4; 145 | return true; // Wall! 146 | } 147 | return false; // No wall 148 | } 149 | 150 | float Controller::get_max_sensor_range() 151 | { 152 | return sensor_range_max; 153 | } 154 | void Controller::set_max_sensor_range(float r) 155 | { 156 | sensor_range_max = r; 157 | } -------------------------------------------------------------------------------- /sw/simulation/simulation_thread.h: -------------------------------------------------------------------------------- 1 | #ifndef SIMULATION_THREAD_H 2 | #define SIMULATION_THREAD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include // system, NULL, EXIT_FAILURE 9 | #include 10 | #include // std::stringstream, std::stringbuf 11 | #include 12 | #include 13 | #include 14 | 15 | #include "main.h" 16 | #include "randomgenerator.h" 17 | #include "terminalinfo.h" 18 | #include "agent_thread.h" 19 | #include "drawingparams.h" 20 | #include "settings.h" 21 | #include "environment.h" 22 | #include "fitness_functions.h" 23 | #include "fifo.h" 24 | 25 | /** 26 | * Extract the number of agents from the argument list. 27 | * Else, return an error. 28 | * 29 | * @param argc Number of arguments from terminal input when launching swarmulator 30 | * @param argv Content of arguments from terminal input when launching swarmulator 31 | */ 32 | void read_argv(int argc, char *argv[]) 33 | { 34 | if (argc <= 1) { 35 | terminalinfo::error_msg("Please specify the number of agents."); 36 | } else { 37 | nagents = std::stoi(argv[1]); 38 | } 39 | } 40 | 41 | /** 42 | * This function initiates the simulation. 43 | * All agents in the beginning initiate randomly with a mean position around the (0,0) point. 44 | * Once the vector of agents is created, each agent is launched in a separate thread. 45 | * 46 | * @param argc Number of arguments from terminal input when launching swarmulator 47 | * @param argv Content of arguments from terminal input when launching swarmulator 48 | */ 49 | void main_simulation_thread(int argc, char *argv[], std::string id) 50 | { 51 | terminalinfo::info_msg("Simulation started."); 52 | read_argv(argc, argv); // Read the number of agents from the argument input 53 | random_generator rg; 54 | fifo f(id); // Open FIFO pipe 55 | 56 | 57 | // Generate the random initial positions with (0,0) mean and 0.5 standard deviation 58 | if (nagents > 0) { 59 | float spread = environment.limits(); // Default 60 | std::vector x0 = rg.uniform_float_vector(nagents, -spread, spread); 61 | std::vector y0 = rg.uniform_float_vector(nagents, -spread, spread); 62 | std::vector t0 = rg.uniform_float_vector(nagents, -M_PI, M_PI); 63 | 64 | // Check whether the robot is in the area, else fix. 65 | // Define rays extending beyond the maximum limits of the area (d) in all 4 directions, and use this to check whether the area is valid. 66 | std::vector> d(4); 67 | d[0] = {0., spread * 1.3f}; // 1.3 is arbitrary, just something > 1.0 68 | d[1] = {0., -spread * 1.3f}; 69 | d[2] = { spread * 1.3f, 0.}; 70 | d[3] = { -spread * 1.3f, 0.}; 71 | 72 | uint16_t ID = 0; 73 | while (ID < nagents) { 74 | bool location_invalid = false; 75 | for (uint16_t dir = 0; dir < d.size(); dir++) { 76 | std::vector s_n = {x0[ID], y0[ID]}; 77 | if (environment.valid(ID, s_n, d[dir])) { 78 | location_invalid = true; 79 | break; // An agent initialized outside of valid area was found. Proceed to fix it. 80 | } 81 | } 82 | 83 | // Get a new location if invalid, else move on to test the next agent. 84 | if (location_invalid) { 85 | x0[ID] = rg.uniform_float(-spread, spread); 86 | y0[ID] = rg.uniform_float(-spread, spread); 87 | } else { 88 | ID++; 89 | } 90 | } 91 | 92 | // Generate the agents in the initial positions 93 | for (uint16_t ID = 0; ID < nagents; ID++) { 94 | // Initial state vector 95 | // [position_x, position y, vel_x=0, vel_y=0, acc_x=0, acc_y=0, psi, psi_rate] 96 | std::vector state = {x0[ID], y0[ID], 0.0, 0.0, 0.0, 0.0, t0[ID], 0.0}; 97 | create_new_agent(ID, state); // Create a new agent 98 | } 99 | } 100 | 101 | // Keep global clock running until swarmulator quits. 102 | // The clock is only used by the animation and the logger, 103 | // which use the clock of the first robot as a reference. 104 | // Robots to do not share this clock and operate in a detached manner. 105 | while (program_running) { 106 | if (!paused) { 107 | // Runtime finish evolution 108 | 109 | // If the parameter time_limit is set to 0 (in conf/parameters.xml) then the simulation runs indefinitely 110 | // Otherwise the simulation will quit after that time has passed, using the clock of the 0th robot as a reference. 111 | if (param->time_limit() > 0.0) { 112 | 113 | // Quit after a certain amount of time 114 | if (simtime_seconds > param->time_limit()) { 115 | 116 | mtx.lock(); // Lock main mutex 117 | mtx_env.lock(); // Lock environment mutex 118 | terminalinfo::debug_msg("Sending message"); 119 | f.send(evaluate_fitness()); // Send FIFO message 120 | mtx_env.unlock(); // Unlock environment mutex 121 | mtx.unlock(); // Unlock main mutex 122 | 123 | program_running = false; // Get ready to quit 124 | } 125 | } 126 | } 127 | }; 128 | 129 | } 130 | #endif /*SIMULATION_THREAD_H*/ 131 | -------------------------------------------------------------------------------- /sw/animation/draw.cpp: -------------------------------------------------------------------------------- 1 | #include "draw.h" 2 | #include "trigonometry.h" 3 | #include 4 | #include "fitness_functions.h" 5 | 6 | void draw::data() 7 | { 8 | glRasterPos2f((-3.9 / zoom_scale - center_x), (-3.9 / zoom_scale - center_y)); 9 | glColor3ub(255, 255, 255); // White 10 | std::stringstream ss; 11 | ss << "Time[s]:" << simtime_seconds << " \t" << "Fitness: " << evaluate_fitness(); 12 | glutBitmapString(GLUT_BITMAP_8_BY_13, (unsigned char *)ss.str().c_str()); 13 | } 14 | 15 | void draw::axis_label() 16 | { 17 | glRasterPos2f(3.9 / zoom_scale - center_x, 0.1 / zoom_scale); 18 | glutBitmapString(GLUT_BITMAP_8_BY_13, (unsigned char *)std::string("E").c_str()); 19 | glRasterPos2f(0.1 / zoom_scale, 3.9 / zoom_scale - center_y); 20 | glutBitmapString(GLUT_BITMAP_8_BY_13, (unsigned char *)std::string("N").c_str()); 21 | } 22 | 23 | void draw::agent_number(const uint16_t &ID) 24 | { 25 | glRasterPos2f(-0.01, 0.035); 26 | glColor3f(1.0, 1.0, 1.0); // Background color 27 | 28 | std::stringstream ss; 29 | ss << (int)ID; 30 | glutBitmapString(GLUT_BITMAP_8_BY_13, (unsigned char *)ss.str().c_str()); 31 | } 32 | 33 | void draw::triangle(const float &scl) 34 | { 35 | glPushMatrix(); 36 | 37 | glBegin(GL_POLYGON); 38 | glColor3ub(200, 000, 000); // Red 39 | glVertex2f(-1 * scl, 1 * scl); 40 | glVertex2f(-1 * scl, -1 * scl); 41 | glVertex2f(2.0 * scl, 0 * scl); 42 | glEnd(); 43 | 44 | glColor3ub(255, 255, 255); // White 45 | glPopMatrix(); 46 | } 47 | 48 | void draw::circle(const float &d) 49 | { 50 | float angle, x, y; 51 | glPushMatrix(); 52 | glBegin(GL_POLYGON); 53 | glColor3ub(200, 000, 000); // Redish 54 | for (int i = 0; i <= 10; i++) { // Resolution 55 | angle = 2 * M_PI * i / 10; 56 | x = (d * yrat) * cos(angle); 57 | y = (d * xrat) * sin(angle); 58 | glVertex2d(x, y); 59 | } 60 | glEnd(); 61 | 62 | glColor3ub(255, 255, 255); // White 63 | glPopMatrix(); 64 | } 65 | 66 | void draw::circle_loop(const float &r) 67 | { 68 | int num_segments = 30; // Resolution 69 | glPushMatrix(); 70 | glLineWidth(1); 71 | glBegin(GL_LINE_LOOP); 72 | for (int i = 0; i < num_segments; i++) { 73 | float theta = 2.0f * M_PI * float(i) / float(num_segments);//get the current angle 74 | float x = r * yrat * cosf(theta); //calculate the x component 75 | float y = r * xrat * sinf(theta); //calculate the y component 76 | glVertex2d(x, y); 77 | } 78 | glEnd(); 79 | glColor3ub(255, 255, 255); // White 80 | glPopMatrix(); 81 | } 82 | 83 | void draw::line(const float &x, const float &y) 84 | { 85 | glPushMatrix(); 86 | glLineWidth(2.5); 87 | glColor3f(1.0, 1.0, 1.0); 88 | glBegin(GL_LINES); 89 | glVertex3f(0.0, 0.0, 0.0); 90 | glVertex3f(x * xrat, -y * yrat, 0); 91 | glEnd(); 92 | glPopMatrix(); 93 | } 94 | 95 | void draw::line(const float &x, const float &y, const float &width) 96 | { 97 | glPushMatrix(); 98 | glLineWidth(width); 99 | glColor3f(1.0, 1.0, 1.0); 100 | glBegin(GL_LINES); 101 | glVertex3f(0.0, 0.0, 0.0); 102 | glVertex3f(x * xrat, -y * yrat, 0); 103 | glEnd(); 104 | glPopMatrix(); 105 | } 106 | 107 | void draw::point() 108 | { 109 | glPointSize(10.0); 110 | glBegin(GL_POINTS); 111 | glVertex3f(0, 0, 0); 112 | glEnd(); 113 | } 114 | 115 | void draw::axes() 116 | { 117 | float lineintensity = 1.0; 118 | glLineWidth(0.5); 119 | glBegin(GL_LINES); 120 | glLineStipple(1, 0xAAAA); // [1] 121 | glEnable(GL_LINE_STIPPLE); 122 | glColor3ub(255 * lineintensity, 255 * lineintensity, 255 * lineintensity); // white 123 | glVertex3f(-1000, 0.0, 0.0); 124 | glVertex3f(1000.0, 0.0, 0.0); 125 | glEnd(); 126 | 127 | glBegin(GL_LINES); 128 | glLineStipple(1, 0xAAAA); // [1] 129 | glEnable(GL_LINE_STIPPLE); 130 | glColor3ub(255 * lineintensity, 255 * lineintensity, 255 * lineintensity); // white 131 | glVertex3f(0.0, -1000.0, 0.0); 132 | glVertex3f(0.0, 1000.0, 0.0); 133 | glEnd(); 134 | glPopAttrib(); 135 | } 136 | 137 | void draw::segment(const float &x0, const float &y0, const float &x1, const float &y1) 138 | { 139 | glLineWidth(5); 140 | float lineintensity = 1.0; 141 | glBegin(GL_LINES); 142 | glColor3ub(128 * lineintensity, 128 * lineintensity, 128 * lineintensity); // white 143 | glVertex3f(x0 * xrat, y0 * yrat, 0.0); 144 | glVertex3f(x1 * xrat, y1 * yrat, 0.0); 145 | glEnd(); 146 | } 147 | 148 | void draw::agent(const uint16_t &ID, const float &x, const float &y, const float &orientation) 149 | { 150 | glPushMatrix(); 151 | glTranslatef(y * xrat, x * yrat, 0.0); // ENU to NED 152 | glRotatef(90.0 - rad2deg(orientation), 0.0, 0, 1); 153 | s[ID]->animation(); // Uses the animation function defined by the agent in use 154 | s[ID]->controller->animation(ID); // Draws additional stuff from the controller, such as sensors 155 | agent_number(ID); 156 | glPopMatrix(); 157 | } 158 | 159 | void draw::velocity_arrow(const uint16_t &ID, const float &x, const float &y, const float &v_x, const float &v_y) 160 | { 161 | glPushMatrix(); 162 | glTranslatef(y * xrat, x * yrat, 0.0); // ENU to NED 163 | glRotatef(90.0, 0.0, 0.0, 1.0); 164 | line(v_x, v_y); 165 | glPopMatrix(); 166 | } 167 | 168 | void draw::food(const float &x, const float &y) 169 | { 170 | glPushMatrix(); 171 | glTranslatef(y * xrat, x * yrat, 0.0); 172 | glRotatef(90, 0.0, 0.0, 1.0); 173 | point(); 174 | glPopMatrix(); 175 | } 176 | -------------------------------------------------------------------------------- /scripts/python/classes/swarmulator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Python API for swarmulator 4 | @author: Mario Coppola, 2020 5 | """ 6 | import os, subprocess, threading, time, random, glob 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | from mpl_toolkits.mplot3d import Axes3D 10 | import concurrent.futures 11 | import tools.fileHandler as fh 12 | 13 | class swarmulator: 14 | '''Python API class for Swarmulator. Allows to interact with swarmulator via Python''' 15 | 16 | def __init__(self, path="../../",verbose=True): 17 | '''Load swarmulator object. Ensure that the correct path to swarmulator is given as input''' 18 | self.path = path 19 | self.verbose = verbose 20 | self._clear_pipes() 21 | 22 | def make(self, controller=None, agent=None, animation=False, logger=False, verbose=False, speed=True, clean=False): 23 | '''Builds swarmulator''' 24 | spd = " -j" if speed else "" 25 | ani = " ANIMATION=ON" if animation else "" 26 | log = " LOG=ON" if logger else "" 27 | vrb = " VERBOSE=ON" if verbose else "" 28 | ctrl = " CONTROLLER="+controller if controller else "" 29 | agnt = " AGENT="+agent if controller else "" 30 | if clean: subprocess.call("cd " + self.path + " && make clean ", shell=True) 31 | subprocess.call("cd " + self.path + " && make" + spd + ani + log + vrb + ctrl + agnt, shell=True) 32 | print("# Done") 33 | 34 | def _clear_pipes(self,folder="/tmp/"): 35 | fileList = glob.glob(folder+"swarmulator_*") # list of all pipes that have been created 36 | # Iterate over the list of filepaths and remove each file 37 | for filePath in fileList: 38 | try: 39 | os.remove(filePath) 40 | except OSError: 41 | print("Error while deleting file") 42 | 43 | def _launch(self, n, run_id): 44 | '''Launches an instance of a swarmulator simulation''' 45 | cmd = "cd " + self.path + " && ./swarmulator " + str(n) + " " + str(run_id) + " &" 46 | # print(cmd) 47 | subprocess.call(cmd, shell=True) 48 | if self.verbose: print("Launched instance of swarmulator with %s robots and pipe ID %s" % (n,run_id)) 49 | 50 | def _get_fitness(self,pipe): 51 | '''Awaits to receive the fitness from a run''' 52 | while not os.path.lexists(pipe): 53 | time.sleep(0.0001) # wait for swarmulator to complete and create the pipe 54 | f = open(pipe).read() # FIFO pipe generated by swarmulator 55 | if self.verbose: print("Received fitness %s from pipe %s" % (str(f),pipe)) 56 | return f 57 | 58 | def run(self, n, run_id=None): 59 | '''Runs swarmulator. If run_id is not specified, it will assign a random id''' 60 | self.run_id = random.randrange(10000000000) if run_id is None else run_id 61 | pipe = "/tmp/swarmulator_" + str(self.run_id) 62 | self._launch(n,run_id=self.run_id) 63 | f = self._get_fitness(pipe) 64 | return f 65 | 66 | def load(self,file=None): 67 | '''Loads the log of a swarmulator run. If id is not specified, it will take the most recent log''' 68 | if file is None: 69 | file = self.path + "/" + fh.get_latest_file(self.path + "/logs/log_*.txt") 70 | log = np.loadtxt(file) 71 | elif '.txt' in file: 72 | log = np.loadtxt(file) 73 | elif '.npz' in file: 74 | log = np.load(file)["log"].astype(float) 75 | else: 76 | raise ValueError("File format unknown!") 77 | return -1 78 | return log 79 | 80 | def plot_log(self, log=None, file=None, time_column=0, id_column=1, x_column=2, y_column=3): 81 | '''Visualizes the log of a swarmulator run''' 82 | if log is None: 83 | if file is None: log = self.load() 84 | else: log = self.load(file) 85 | robots = int(log[:,id_column].max()) 86 | if self.verbose: print("Total number of robots: " + str(robots)) 87 | fig = plt.figure() 88 | ax = fig.gca(projection='3d') 89 | for x in range(1,robots): 90 | d = log[np.where(log[:,id_column] == x)] 91 | ax.plot(d[:,time_column],d[:,x_column],d[:,y_column]) 92 | ax.set_xlabel("Time [s]") 93 | ax.set_ylabel("N [m]") 94 | ax.set_zlabel("E [m]") 95 | plt.show() 96 | 97 | def runtime_setting(self, setting, value): 98 | '''Assigns a value to a runtime setting of conf/parameters.xml''' 99 | s = "xmlstarlet edit -L -u \"/parameters/" + setting + "\" -v \""+ value + "\" " + self.path + "/conf/parameters.xml" 100 | subprocess.call(s, shell=True) 101 | if self.verbose: print("Runtime setting \"" + setting + "\" has been set to \"" + value + "\"") 102 | 103 | def get_runtime_setting(self, setting): 104 | '''Returns the value of a runtime parameter currently specified in swarmulator conf/parameters.xml''' 105 | s = "xmlstarlet sel -t -v" + " \"parameters/" +setting + "\" "+ self.path + "/conf/parameters.xml" 106 | value = subprocess.getoutput(s) 107 | if self.verbose: print("Runtime setting \"" + setting + "\" is \"" + value + "\"") 108 | return value; 109 | 110 | def batch_run(self,n,runs): 111 | '''Runs a batch of parallel simulations in parallel. By being different processes, the simulations can run unobstructed.''' 112 | self._clear_pipes() 113 | if isinstance(n,int): 114 | robots = np.repeat(n,runs) 115 | elif len(n) == 2: 116 | robots = np.random.randint(n[0],n[1],runs) 117 | 118 | if runs == 1: 119 | print("WARNING: Running simulator.batch_run but only using 1 run. Consider just using swarmulator.run instead") 120 | 121 | out = np.zeros(runs) 122 | c = 0 123 | with concurrent.futures.ProcessPoolExecutor() as executor: 124 | for i, f in zip(robots,executor.map(self.run, robots)): 125 | out[c] = float(f) 126 | c += 1 127 | 128 | return out 129 | -------------------------------------------------------------------------------- /sw/simulation/sensors/omniscient_observer.cpp: -------------------------------------------------------------------------------- 1 | #include "omniscient_observer.h" 2 | #include "trigonometry.h" 3 | #include "main.h" 4 | 5 | int compare_index(const void *p1, const void *p2) 6 | { 7 | indexed_array *elem1 = (indexed_array *)p1; 8 | indexed_array *elem2 = (indexed_array *)p2; 9 | 10 | if (elem1->values < elem2->values) { 11 | return -1; 12 | } else if (elem1->values > elem2->values) { 13 | return 1; 14 | } else { 15 | return 0; 16 | } 17 | } 18 | 19 | void array_sortmintomax_index(int length, indexed_array *x) 20 | { 21 | qsort(x, length, sizeof(struct indexed_array), compare_index); 22 | return; 23 | } 24 | 25 | std::vector OmniscientObserver::request_closest(const uint16_t &ID) 26 | { 27 | indexed_array dm[s.size() - 1]; 28 | std::vector ind; 29 | for (uint16_t i = 0; i < s.size(); i++) { 30 | dm[i].values = (sqrt( 31 | pow(s[i]->get_position(0) - s[ID]->get_position(0), 2.0) 32 | + pow(s[i]->get_position(1) - s[ID]->get_position(1), 2.0) 33 | )); 34 | dm[i].index = i; 35 | } 36 | 37 | array_sortmintomax_index(s.size(), dm); 38 | 39 | // Start from one to eliminate youself from the list (because you are 0 distance) 40 | for (uint16_t i = 1; i < s.size(); i++) { 41 | ind.push_back(dm[i].index); 42 | } 43 | 44 | return ind; 45 | } 46 | 47 | std::vector OmniscientObserver::request_closest_inrange(const uint16_t &ID, const float &range) 48 | { 49 | indexed_array dm[s.size() - 1]; 50 | std::vector ind; 51 | for (uint16_t i = 0; i < s.size(); i++) { 52 | dm[i].values = (sqrt( 53 | pow(s[i]->get_position(0) - s[ID]->get_position(0), 2.0) 54 | + pow(s[i]->get_position(1) - s[ID]->get_position(1), 2.0) 55 | )); 56 | dm[i].index = i; 57 | } 58 | 59 | array_sortmintomax_index(s.size(), dm); 60 | 61 | // Start from one to eliminate youself from the list (because you are 0 distance) 62 | for (uint16_t i = 1; i < s.size(); i++) { 63 | if (dm[i].values < range) { 64 | ind.push_back(dm[i].index); 65 | } 66 | } 67 | return ind; 68 | } 69 | 70 | bool OmniscientObserver::check_happy() 71 | { 72 | bool happy = true; 73 | for (uint16_t ID = 0; ID < s.size(); ID++) { 74 | if (!s[ID]->controller->happy) { 75 | return false; 76 | } 77 | } 78 | 79 | return happy; 80 | } 81 | 82 | bool OmniscientObserver::connected_graph_range(const float &range) 83 | { 84 | Graph g(s.size()); 85 | 86 | for (uint16_t i = 0; i < s.size(); i++) { 87 | for (uint16_t j = 0; j < s.size(); j++) { 88 | if (request_distance(i, j) < range) { 89 | g.addEdge(i, j); 90 | } 91 | } 92 | } 93 | 94 | if (g.isConnected()) { 95 | return true; 96 | } 97 | return false; 98 | } 99 | 100 | float OmniscientObserver::get_centroid(const uint16_t &dim) 101 | { 102 | float c = 0; 103 | for (uint16_t i = 0; i < s.size(); i++) { 104 | c += s[i]->get_position(dim) / (float)s.size(); 105 | } 106 | return c; 107 | } 108 | 109 | float OmniscientObserver::request_distance_dim(const uint16_t &ID, const uint16_t &ID_tracked, const uint16_t &dim) 110 | { 111 | return s[ID_tracked]->get_position(dim) - s[ID]->get_position(dim); 112 | } 113 | 114 | float OmniscientObserver::request_distance(const uint16_t &ID, const uint16_t &ID_tracked) 115 | { 116 | float u = 0; 117 | for (uint16_t i = 0; i < 2; i++) { 118 | float dd = s[ID_tracked]->get_position(i) - s[ID]->get_position(i); 119 | u += pow(dd, 2); 120 | } 121 | float noise = rg.gaussian_float(0.0, NOISE_R); 122 | return sqrt(u) + noise; 123 | } 124 | 125 | float OmniscientObserver::own_bearing(const uint16_t &ID) 126 | { 127 | return s[ID]->get_orientation(); 128 | } 129 | 130 | float OmniscientObserver::request_bearing(const uint16_t &ID, const uint16_t &ID_tracked) 131 | { 132 | float noise = rg.gaussian_float(0.0, NOISE_B); 133 | float b = atan2(request_distance_dim(ID, ID_tracked, 1), request_distance_dim(ID, ID_tracked, 0)) + noise; 134 | #if COMMAND_LOCAL 135 | return b - own_bearing(ID); 136 | #else 137 | return b; 138 | #endif 139 | 140 | } 141 | 142 | bool OmniscientObserver::see_if_moving(const uint16_t &ID) 143 | { 144 | return s[ID]->moving; 145 | } 146 | 147 | void OmniscientObserver::relative_location_inrange(const uint16_t ID, const float range, std::vector &r, 148 | std::vector &b) 149 | { 150 | std::vector closest = request_closest_inrange(ID, range); 151 | for (size_t i = 0; i < closest.size(); i++) { 152 | r.push_back(request_distance(ID, closest[i])); 153 | b.push_back(request_bearing(ID, closest[i])); 154 | } 155 | } 156 | 157 | void OmniscientObserver::relative_location(const uint16_t ID, std::vector &r, std::vector &b) 158 | { 159 | std::vector c = request_closest(ID); 160 | for (size_t i = 0; i < c.size(); i++) { 161 | r.push_back(request_distance(ID, c[i])); 162 | b.push_back(request_bearing(ID, c[i])); 163 | } 164 | } 165 | 166 | bool OmniscientObserver::sense_food(const uint16_t ID, uint16_t &food_ID, float range) 167 | { 168 | mtx_env.lock_shared(); 169 | for (uint16_t i = 0; i < environment.food.size(); i++) { 170 | float u = 0; 171 | for (size_t j = 0; j < 2; j++) { 172 | float dd = s[ID]->get_position(j) - environment.food[i][j]; 173 | u += pow(dd, 2); 174 | } 175 | if (sqrt(u) < range) { 176 | food_ID = i; 177 | mtx_env.unlock_shared(); 178 | return true; 179 | } 180 | } 181 | mtx_env.unlock_shared(); 182 | return false; 183 | } 184 | 185 | void OmniscientObserver::beacon(const uint16_t ID, float &r, float &b) 186 | { 187 | mtx_env.lock_shared(); 188 | float x = environment.beacon[0] - s[ID]->get_position(0); 189 | float y = environment.beacon[1] - s[ID]->get_position(1); 190 | cart2polar(x, y, r, b); 191 | #if COMMAND_LOCAL 192 | b -= own_bearing(ID); 193 | #endif 194 | mtx_env.unlock_shared(); 195 | } 196 | -------------------------------------------------------------------------------- /scripts/python/classes/evolution.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Simulate the aggregation and optimize the behavior 4 | @author: Mario Coppola, 2020 5 | """ 6 | import random, sys, pickle, matplotlib 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | from deap import base, creator, tools 10 | matplotlib.rc('text', usetex=True) 11 | 12 | class evolution: 13 | '''Wrapper around the DEAP package to run an evolutionary process with just a few commands''' 14 | 15 | def __init__(self): 16 | '''Itilize the DEAP wrapper''' 17 | creator.create("FitnessMax", base.Fitness, weights=(1.0,)) 18 | creator.create("Individual", list, fitness=creator.FitnessMax) 19 | 20 | def setup(self, fitness_function_handle, constraint=None, GENOME_LENGTH = 20, POPULATION_SIZE = 100, P_CROSSOVER = 0.5, P_MUTATION = 0.2): 21 | '''Set up the parameters''' 22 | # Set the main variables 23 | self.GENOME_LENGTH = GENOME_LENGTH 24 | self.POPULATION_SIZE = POPULATION_SIZE 25 | self.P_CROSSOVER = P_CROSSOVER 26 | self.P_MUTATION = P_MUTATION 27 | 28 | # Set the lower level parameters 29 | self.toolbox = base.Toolbox() 30 | self.toolbox.register("attr_float", random.random) 31 | self.toolbox.register("individual", tools.initRepeat, creator.Individual, self.toolbox.attr_float, self.GENOME_LENGTH) 32 | self.toolbox.register("population", tools.initRepeat, list, self.toolbox.individual) 33 | self.toolbox.register("evaluate", fitness_function_handle) 34 | self.toolbox.register("mate", tools.cxTwoPoint) # Mating method 35 | self.toolbox.register("mutate", tools.mutFlipBit, indpb=0.05) # Mutation method 36 | self.toolbox.register("select", tools.selTournament, tournsize=3) # Selection method 37 | if constraint is not None: self.toolbox.decorate("evaluate", tools.DeltaPenalty(constraint, 7)) 38 | self.stats = [] # Initialize stats vector 39 | 40 | def store_stats(self, population, iteration=0): 41 | '''Store the current stats and return a dict''' 42 | fitnesses = [ individual.fitness.values[0] for individual in population ] 43 | return { 44 | 'g': iteration, 45 | 'mu': np.mean(fitnesses), 46 | 'std': np.std(fitnesses), 47 | 'max': np.max(fitnesses), 48 | 'min': np.min(fitnesses) 49 | } 50 | 51 | def disp_stats(self,iteration=0): 52 | '''Print the current stats''' 53 | print(">> gen = %i, mu = %.2f, std = %.2f, max = %.2f, min = %.2f" % 54 | (self.stats[iteration]['g'], 55 | self.stats[iteration]['mu'], 56 | self.stats[iteration]['std'], 57 | self.stats[iteration]['max'], 58 | self.stats[iteration]['min'])) 59 | 60 | def plot_evolution(self,figurename=None): 61 | '''Plot the evolution outcome''' 62 | plt.style.use('seaborn-whitegrid') 63 | plt.plot(range(1, len(self.stats)+1), [ s['mu'] for s in self.stats ]) 64 | plt.title('Average fitness per iteration') 65 | plt.xlabel('Iterations') 66 | plt.ylabel('Fitness') 67 | plt.fill_between(range(1, len(self.stats)+1), 68 | [ s['mu']-s['std'] for s in self.stats ], 69 | [ s['mu']+s['std'] for s in self.stats ], 70 | color='gray', alpha=0.2) 71 | plt.xlim(0,len(self.stats)) 72 | plt.savefig(figurename) if figurename is not None else plt.show() 73 | 74 | def evolve(self, generations=100, verbose=False, population=None, checkpoint=None): 75 | '''Run the evolution. Use checkpoint="filename.pkl" to save the status to a file after each generation, just in case.''' 76 | random.seed() # Use random seed 77 | pop = population if population is not None else self.toolbox.population(n=self.POPULATION_SIZE) 78 | if verbose: print('{:=^40}'.format(' Start of evolution ')) 79 | 80 | # Evaluate the initial population 81 | fitnesses = list(map(self.toolbox.evaluate, pop)) 82 | for ind, fit in zip(pop, fitnesses): 83 | ind.fitness.values = fit 84 | 85 | # Evolve! 86 | g = len(self.stats) # Number of generations 87 | gmax = len(self.stats) + generations 88 | while g < gmax: 89 | # Offspring 90 | offspring = self.toolbox.select(pop, len(pop)) 91 | offspring = list(map(self.toolbox.clone, offspring)) 92 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 93 | if random.random() < self.P_CROSSOVER: self.toolbox.mate(child1, child2) 94 | del child1.fitness.values 95 | del child2.fitness.values 96 | 97 | # Mutate 98 | for mutant in offspring: 99 | if random.random() < self.P_MUTATION: self.toolbox.mutate(mutant) 100 | del mutant.fitness.values 101 | 102 | # Evaluate the individuals with an invalid fitness 103 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 104 | fitnesses = map(self.toolbox.evaluate, invalid_ind) 105 | for ind, fit in zip(invalid_ind, fitnesses): 106 | ind.fitness.values = fit 107 | 108 | pop[:] = offspring # Replace population 109 | self.stats.append(self.store_stats(pop, g)) # Store stats 110 | if verbose: self.disp_stats(g) 111 | 112 | if checkpoint is not None: self.save(checkpoint, pop=pop, gen=g, stats=self.stats) 113 | 114 | g += 1 115 | 116 | # Store oucomes 117 | self.pop = pop 118 | self.best_ind = self.get_best() 119 | self.g = g 120 | 121 | # Save to checkpoint file 122 | if checkpoint is not None: self.save(checkpoint) 123 | 124 | # Display outcome 125 | if verbose: 126 | print('{:=^40}'.format(' End of evolution ')) 127 | print("Best individual is %s, %s" % (self.best_ind, self.best_ind.fitness.values)) 128 | 129 | return pop 130 | 131 | def save(self,filename,pop=None,gen=None,stats=None): 132 | '''Save the current status in a pkl file''' 133 | p = self.pop if pop is None else pop 134 | g = self.g if gen is None else gen 135 | s = self.stats if stats is None else stats 136 | cp = dict(population=p, generation=g, stats=s) 137 | with open(filename+".pkl", "wb") as cp_file: 138 | pickle.dump(cp, cp_file) 139 | 140 | def load(self,filename): 141 | '''Load the status from a pkl file''' 142 | with open(filename+".pkl", "rb") as cp_file: 143 | cp = pickle.load(cp_file) 144 | self.stats = cp["stats"] 145 | self.g = cp["generation"] 146 | self.pop = cp["population"] 147 | return self.pop 148 | 149 | def get_best(self,pop=None): 150 | '''Returns the fittest element of a population''' 151 | p = self.pop if pop is None else pop 152 | return tools.selBest(p,1)[0] 153 | -------------------------------------------------------------------------------- /sw/simulation/controllers/behavior_tree/bt/btFile.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "btFile.h" 9 | 10 | namespace BT 11 | { 12 | 13 | composite *loadFile(const char *file) 14 | { 15 | node *root; 16 | nodes tasks; 17 | 18 | std::string line; 19 | std::ifstream myfile; 20 | 21 | size_t i_tab, currentTab = 0; 22 | 23 | myfile.open(file); 24 | if (myfile.is_open()) { 25 | // GetfileInfo 26 | 27 | // Get root node 28 | std::getline(myfile, line); 29 | line.erase(std::remove(line.begin(), line.end(), ' '), line.end()); // remove white spaces 30 | root = decodeLine(line); 31 | tasks.push_back(root); 32 | 33 | // Read rest of BT 34 | while (std::getline(myfile, line)) { 35 | line.erase(std::remove(line.begin(), line.end(), ' '), line.end()); // remove white spaces 36 | // Determine depth 37 | i_tab = std::count(line.begin(), line.end(), '\t'); 38 | 39 | // New sub task 40 | if (i_tab > currentTab) { 41 | tasks.push_back(newTask(tasks.back(), line)); 42 | currentTab = i_tab; 43 | continue; // read next line 44 | } 45 | 46 | // End of sub tasks, move to new task 47 | else if (i_tab < currentTab) { 48 | while (i_tab != currentTab) { 49 | tasks.pop_back(); 50 | --currentTab; 51 | } 52 | } 53 | // add new task 54 | tasks.pop_back(); 55 | tasks.push_back(newTask(tasks.back(), line)); 56 | //continue to next line 57 | } 58 | myfile.close(); 59 | } else { 60 | printf("ERROR! Unable to open behaviour tree file. Please indicate it in the parameters.xml file, under \n%s\n\n", 61 | file); 62 | exit(-1); 63 | } 64 | return (composite *) root; 65 | } 66 | 67 | bool saveFile(const char *file, composite *task) 68 | { 69 | std::ofstream myfile(file); 70 | 71 | if (myfile.is_open()) { 72 | myfile << codeLine(task) << std::endl; 73 | if (task->type.compare("composite") == 0) { 74 | writeComposite(myfile, task, 1); 75 | } 76 | 77 | myfile.close(); 78 | } else { 79 | std::cout << "Unable to open file" << std::endl; 80 | return false; 81 | } 82 | return true; 83 | } 84 | 85 | void writeComposite(std::ofstream &myfile, composite *parent, size_t tabs) 86 | { 87 | node *task; 88 | nodes children; 89 | 90 | children = parent->getChildren(); 91 | 92 | if (myfile.is_open()) { 93 | for (size_t i = 0; i < children.size(); i++) { 94 | task = children[i]; // write last child first 95 | for (size_t i = 0; i < tabs; i++) { 96 | myfile << '\t'; 97 | } 98 | 99 | // print task line 100 | myfile << codeLine(task) << std::endl; 101 | 102 | if (task->type.compare("composite") == 0) { 103 | writeComposite(myfile, (composite *) task, tabs + 1); 104 | } 105 | } 106 | } 107 | } 108 | 109 | /* 110 | Create new child and add to parent composite 111 | */ 112 | node *newTask(node *Parent, std::string line) 113 | { 114 | // decode line and create new task 115 | composite *parent = (composite *)Parent; 116 | node *task = decodeLine(line); 117 | 118 | parent->addChild(task); 119 | 120 | return task; 121 | } 122 | 123 | /* 124 | Decode BT task from HTML style string line 125 | */ 126 | node *decodeLine(std::string line) 127 | { 128 | // std::stringstream ss(line); 129 | // std::string current; 130 | node *task; 131 | 132 | std::string param, function, vars; 133 | std::vector values; 134 | 135 | // temporary place holders 136 | std::size_t start, end, pos1, pos2; 137 | 138 | // find function type 139 | start = line.find("") + 10; 140 | end = line.find_first_of("<", start); 141 | function = line.substr(start, end - start); 142 | 143 | // find task type 144 | start = line.find("") + 8; 145 | end = line.find_first_of("<", start); 146 | param = line.substr(start, end - start); 147 | 148 | // find task variables 149 | start = line.find("") + 6; 150 | end = line.find_first_of("<", start); 151 | vars = line.substr(start, end - start); 152 | 153 | if ((start - end) > 0) { 154 | pos1 = 0; pos2 = 0; 155 | while (pos1 < vars.length()) { 156 | if (vars.find(",", pos1) == std::string::npos) { 157 | pos2 = vars.length(); 158 | } else { 159 | pos2 = vars.find(",", pos1); 160 | } 161 | values.push_back(atof((vars.substr(pos1, pos2 - pos1)).c_str())); 162 | pos1 = pos2 + 1; 163 | } 164 | } 165 | 166 | if (param.compare("action") == 0) { 167 | task = getAction(function, values); 168 | } else if (param.compare("condition") == 0) { 169 | task = getCondition(function, values); 170 | } else if (param.compare("composite") == 0) { 171 | task = getComposite(function); 172 | } else { 173 | std::cout << "Unidentified BTtype: " << param << std::endl; 174 | } 175 | 176 | /* 177 | else 178 | { 179 | if (param.compare("Action") == 0) 180 | task = getAction( function ); 181 | else if (param.compare("Condition") == 0) 182 | task = getCondition( function ); 183 | else if (param.compare("composite") == 0) 184 | task = getComposite( function ); 185 | else 186 | std::cout << "Unidentified BTtype: "<< param<") + 6; 191 | end = line.find_first_of("<", start + 1); 192 | param = line.substr(start, end - start); 193 | task->name = param; 194 | 195 | return task; 196 | } 197 | 198 | /* 199 | Code BT task to string line 200 | */ 201 | std::string codeLine(node *task) 202 | { 203 | std::stringstream line; 204 | 205 | size_t k_vars = task->vars.size(); 206 | 207 | line << ""; 208 | line << task->type; 209 | line << ""; 210 | line << task->function; 211 | line << ""; 212 | for (size_t i = 0; i < k_vars; i++) { 213 | if (i > 0) { 214 | line << "," << task->vars[i]; 215 | } else { 216 | line << task->vars[i]; 217 | } 218 | } 219 | line << ""; 220 | line << task->name; 221 | line << ""; 222 | 223 | return line.str(); 224 | } 225 | 226 | } 227 | --------------------------------------------------------------------------------