├── .gitignore ├── ArchitectureDetail_corrected.svg ├── README.md ├── WalkingPatterns_paper.png ├── architecture.png ├── docker ├── Dockerfile ├── bio_walk_paths.sh ├── bio_walk_setup.sh ├── build.sh └── run.sh ├── matsuoka_walk ├── __init__.py ├── cristiano2014.py ├── cristiano2014_kf_limits.py ├── cristiano2014_network.py ├── cristiano2014_network_robot.py ├── cristiano2014_phase_reset.py ├── fitness.py ├── ga.py ├── ga_2.py ├── ga_3.py ├── ga_4.py ├── ga_5.py ├── ga_5_1.py ├── ga_test.py ├── gait_eval.py ├── gait_eval_result.py ├── log.py ├── matsuoka2011.py ├── matsuoka_angle_fb_env.py ├── matsuoka_ddpg.py ├── matsuoka_env.py ├── monitor.py ├── monitor_1.py ├── monitor_eval.py ├── oscillator.py ├── oscillator_2.py ├── oscillator_2_1.py ├── oscillator_2_1_eval.py ├── oscillator_3.py ├── oscillator_3_eval.py ├── oscillator_3_test_yaw.py ├── oscillator_3_thread.py ├── oscillator_4.py ├── oscillator_5.py ├── oscillator_5_1.py ├── oscillator_5_1_eval.py ├── oscillator_5_eval.py ├── robots.py ├── ros.py ├── single.py ├── slow_angle_test.py └── static_deviation_test.py ├── plot_scripts ├── __init__.py ├── analyze_log.py ├── benchmark_fitness_function_surface_plots.py ├── ga_plot_paper.py ├── gait_eval_boxplot.py ├── gait_eval_boxplot_from_csv.py ├── gait_eval_param_correlation.py ├── genetic_algorithm_multi_plots_rows.py ├── genetic_algorithm_multi_plots_rows_and_cols.py ├── genetic_algorithm_plots.py ├── keras_nw_graph.py ├── rl_deviation_distance_tests_boxplot.py ├── rl_deviation_distance_tests_boxplot_paper.py ├── rl_reward_deviation_distance_vs_episodes.py ├── rl_reward_deviation_distance_vs_episodes_subplots.py └── rl_reward_deviation_distance_vs_episodes_subplots_paper.py ├── shell_scripts ├── asus_20171005.sh ├── find_acronyms.py ├── logreader.sh ├── run_anglefeedback.sh ├── run_openloop.sh ├── run_phasereset.sh ├── svg_to_eps.sh └── wtmpc_best_chromosomes.txt └── vrep_scripts ├── remoteApiConnections.txt ├── start_vrep.sh ├── start_vrep_headless.sh └── torso_11_respondable.lua /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | 27 | # PyInstaller 28 | # Usually these files are written by a python script from a template 29 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 30 | *.manifest 31 | *.spec 32 | 33 | # Installer logs 34 | pip-log.txt 35 | pip-delete-this-directory.txt 36 | 37 | # Unit test / coverage reports 38 | htmlcov/ 39 | .tox/ 40 | .coverage 41 | .coverage.* 42 | .cache 43 | nosetests.xml 44 | coverage.xml 45 | *,cover 46 | .hypothesis/ 47 | 48 | # Translations 49 | *.mo 50 | *.pot 51 | 52 | # Django stuff: 53 | *.log 54 | local_settings.py 55 | 56 | # Flask stuff: 57 | instance/ 58 | .webassets-cache 59 | 60 | # Scrapy stuff: 61 | .scrapy 62 | 63 | # Sphinx documentation 64 | docs/_build/ 65 | 66 | # PyBuilder 67 | target/ 68 | 69 | # IPython Notebook 70 | .ipynb_checkpoints 71 | 72 | # pyenv 73 | .python-version 74 | 75 | # celery beat schedule file 76 | celerybeat-schedule 77 | 78 | # dotenv 79 | .env 80 | 81 | # virtualenv 82 | venv/ 83 | ENV/ 84 | 85 | # Spyder project settings 86 | .spyderproject 87 | 88 | # Rope project settings 89 | .ropeproject 90 | 91 | # Do not upload WTM robot model 92 | models/ 93 | scenes/ 94 | !vrep_scenes/ 95 | 96 | *.*~ 97 | .idea/ 98 | !bio_walk/topology_schema/ 99 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Hierarchical Control for Bipedal Locomotion using Central Pattern Generators and Neural Networks 2 | (Master of Science Thesis, Intelligent Adaptive Systems, University of Hamburg) 3 | 4 | A biologically inspired, hierarchical bipedal locomotion controller for robots. At the lower level, a CPG 5 | network (based on [this work](https://link.springer.com/chapter/10.1007/978-3-319-03413-3_39)) with feedback pathways controls the individual joints. The parameters 6 | of the CPG network are optimized by a genetic algorithm. At the higher level, a 7 | neural network modulates the output of the CPG network in order to optimize the 8 | robot’s movements with respect to an overall objective. In this case, the objective 9 | is to minimize the lateral deviation while walking which may occur due to slippage or due to an imperfect robot model. The neural network is trained 10 | using the [deep deterministic policy gradient algorithm](https://arxiv.org/pdf/1509.02971.pdf) (a deep reinforcement learning algorithm). This work was carried out using the [NICO humanoid robot](https://www.inf.uni-hamburg.de/en/inst/ab/wtm/research/neurobotics/nico.html). 11 | 12 | ![Correction of deviation](WalkingPatterns_paper.png) 13 | *The hierarchical controller can minimize the lateral deviation, even in the presence of systematic and non-systematic errors. The robot with the red colored path uses only the CPG network. For the blue-paths the hierarchical controller was used. The highlighted case (4th from left) shows the best performing hyperparameter setup.* 14 | 15 | ## Paper 16 | 17 | The paper presented at ICDL-Epirob 2019 can be viewed [here (official)](https://ieeexplore.ieee.org/document/8850683) or [here (arxiv)](https://arxiv.org/abs/1909.00732). 18 | 19 | ## Thesis 20 | 21 | My MSc thesis can be viewed or downloaded from [here](http://edoc.sub.uni-hamburg.de/informatik/volltexte/2018/237/). 22 | 23 | ## Video 24 | 25 | A small explanation of the architecture and videos of the robot's walking motion can be found [here](https://www.youtube.com/watch?v=4c64rKhj72E). 26 | 27 | ## Note 28 | 29 | * The NICO robot's VREP model and the associated Python API can be found at [this link](https://www.inf.uni-hamburg.de/en/inst/ab/wtm/research/neurobotics/nico.html) under resources. The folders motor_configs, nicomotion and vrep_scenes are empty in this repository but the relevant files should be obtained from the links mentioned above. 30 | 31 | ## Architecture 32 | 33 | ![Architecture](ArchitectureDetail_corrected.svg) 34 | 35 | ## Important folders/modules 36 | 37 | 1. docker: Scripts for setting up the Docker environment (instructions are provided in a later section of this readme). 38 | 2. matsuoka_walk: All necessary scripts for the hierarchical controller (details of important scripts are provided in the next section). 39 | 3. motor_configs: Motor configuration files for the NICO robot (empty at present) 40 | 4. nicomotion: API for the NICO robot's movements. Some functionality has been added to the default API. (empty at present) 41 | 5. plot_scripts: Scripts used for creating the plots used in the thesis 42 | 6. plots: Plots generated by the plot scripts. 43 | 7. shell_scripts: Utility shell scripts. 44 | 8. vrep_scenes: VREP scenes with the NICO robot, used in the experiments. 45 | 9. vrep_scripts: Scripts for the VREP simulator 46 | 47 | ## Important scripts in matsuoka_walk 48 | 49 | 1. cristiano2014_network.py: Implementation of the Matsuoka CPG network proposed by Cristiano et al. 50 | 2. fitness.py: Different types of fitness functions used by the genetic algorithm. 51 | 3. ga_2.py: Script used to run the GA for open loop. 52 | 4. ga_3.py: Script used to run the GA for angle feedback. 53 | 5. ga_5.py: Script used to run the GA for phase reset (with ROS) - This is not used in the final results. 54 | 6. ga_5_1.py: Script used to run the GA for phase reset (without ROS). 55 | 7. gait_eval.py: Used for the gait evaluation experiments 56 | 8. log.py: Provides the infrastructure for logging. 57 | 9. matsuoka2011.py: Implementation of a solitary Matsuoka oscillator. 58 | 10. matsuoka_ddpg.py: Script for the DDPG algorithm. 59 | 11. matsuoka_env.py: Script for creating an OpenAI Gym environment for the DDPG algorithm. 60 | 12. monitor.py: Monitors the state of the robot, also used to detect falls. 61 | 13. monitor_1.py: Monitoring script with force sensing capability. 62 | 14. monitor_eval.py: Monitoring script used for gait evaluation. 63 | 15. oscillator*.py: Implementation of the Matsuoka CPG network for various setups. This is used by the GA and DDPG scripts. 64 | 16. robots.py: Wrapper scripts for different robots. 65 | 17. ros.py: Provides ROS functionality. 66 | 67 | 68 | ## Python Virtual Environment setup (recommended) 69 | 70 | 1. Set the $HOME location to where the repository can be stored: 71 | 72 | ```bash 73 | ### Change the path as required 74 | export HOME=/data/sayantan 75 | ``` 76 | 77 | 2. Clone the repository (or copy it from the disk) 78 | 79 | ```bash 80 | cd $HOME 81 | mkdir -p $HOME/computing/repositories/ 82 | cd $HOME/computing/repositories/ 83 | git clone https://github.com/sayantanauddy/hierarchical_bipedal_controller.git 84 | ``` 85 | 86 | 3. Download VREP 87 | 88 | ```bash 89 | cd $HOME 90 | mkdir $HOME/computing/simulators/ 91 | cd $HOME/computing/simulators/ 92 | # Download 93 | wget http://coppeliarobotics.com/files/V-REP_PRO_EDU_V3_4_0_Linux.tar.gz 94 | # Extract 95 | tar -xvf V-REP_PRO_EDU_V3_4_0_Linux.tar.gz 96 | ``` 97 | 98 | 3. Copy the VREP scripts 99 | 100 | ```bash 101 | cd $HOME/computing/repositories/hierarchical_bipedal_controller 102 | cp start_vrep.sh $HOME/computing/simulators/V-REP_PRO_EDU_V3_4_0_Linux/ 103 | cp remoteApiConnections.txt $HOME/computing/simulators/V-REP_PRO_EDU_V3_4_0_Linux/ 104 | cd $HOME/computing/simulators/V-REP_PRO_EDU_V3_4_0_Linux/ 105 | chmod a+x start_vrep.sh 106 | ``` 107 | 108 | 4. Create the log folder 109 | 110 | ```bash 111 | mkdir -p $HOME/.bio_walk/logs/ 112 | ``` 113 | 114 | 5. Create the virtual environment 115 | 116 | ```bash 117 | cd $HOME 118 | virtualenv --system-site-packages $HOME/matsuoka_virtualenv 119 | # Activate the virtual environment 120 | source $HOME/matsuoka_virtualenv/bin/activate 121 | ``` 122 | 123 | 6. Add the code location to PYTHONPATH 124 | 125 | ```bash 126 | export PYTHONPATH=$PYTHONPATH:$HOME/computing/repositories/hierarchical_bipedal_controller/nicomotion 127 | export PYTHONPATH=$PYTHONPATH:$HOME/computing/repositories/hierarchical_bipedal_controller 128 | ``` 129 | 130 | 7. Install the dependencies 131 | 132 | ```bash 133 | # numpy, matplotlib should also be installed 134 | pip install pypot 135 | pip install poppy-humanoid 136 | pip install deap 137 | pip install --upgrade tensorflow 138 | pip install keras 139 | pip install gym 140 | pip install h5py 141 | ``` 142 | 143 | 8. Start VREP in a terminal 144 | 145 | ```bash 146 | cd $HOME/computing/simulators/V-REP_PRO_EDU_V3_4_0_Linux 147 | ./start_vrep.sh 148 | ``` 149 | 150 | 9. Run the necessary script. For example ga_3.py to run the genetic algorithm or matsuoka_ddpg.py to run the DDPG training. 151 | 152 | ## Citation 153 | 154 | @INPROCEEDINGS{auddy2019, 155 | author={S. {Auddy} and S. {Magg} and S. {Wermter}}, 156 | booktitle={2019 Joint IEEE 9th International Conference on Development and Learning and Epigenetic Robotics (ICDL-EpiRob)}, 157 | title={Hierarchical Control for Bipedal Locomotion using Central Pattern Generators and Neural Networks}, 158 | year={2019}, 159 | volume={}, 160 | number={}, 161 | pages={13-18}, 162 | doi={10.1109/DEVLRN.2019.8850683}, 163 | ISSN={2161-9484}, 164 | month={Aug},} 165 | 166 | @Mastersthesis{AuddyMscThesis2017, 167 | Title = {Hierarchical Control for Bipedal Locomotion using Central Pattern Generators and Neural Networks}, 168 | Author = {Auddy, Sayantan}, 169 | school = {Universit{\"a}t Hamburg, Hamburg, Germany}, 170 | language = {English}, 171 | Year = {2017}, 172 | month = {Dec} 173 | } 174 | 175 | ## Miscelleneous 176 | 1. Ros topic for force sensors: '/nico_feet_forces', message type:'std_msgs/String' 177 | 2. Force vectors for l1,l2,l3,l4,r1,r2,r3,r4 are concatanated into a single string and published 178 | 3. The following were added to .bashrc `source /opt/ros/indigo/setup.bash` and `source ~/catkin_ws/devel/setup.bash` 179 | (Steps 4-6 are needed to run ROS code outside the catkin workspace) 180 | 4. VREP specific information: 181 | * VREP should ideally be invoked using the command LC_NUMERIC=en_US.UTF-8 ./vrep.sh to make sure that '.' is used as the decimal separator and not ',' 182 | * custom remoteApiConnections.txt - Defines the ports for communicating with VREP (see example in vrep_scripts folder). 183 | * If using ROS with VREP - [Create VREP ROS bridge](http://www.coppeliarobotics.com/helpFiles/en/rosTutorialIndigo.htm) 184 | 185 | 186 | -------------------------------------------------------------------------------- /WalkingPatterns_paper.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sayantanauddy/hierarchical_bipedal_controller/ef1dfbd7f8e3ced3c574d15bcddca634e4d9de6c/WalkingPatterns_paper.png -------------------------------------------------------------------------------- /architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sayantanauddy/hierarchical_bipedal_controller/ef1dfbd7f8e3ced3c574d15bcddca634e4d9de6c/architecture.png -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:indigo 2 | 3 | # Arguments 4 | ARG user 5 | ARG uid 6 | ARG home 7 | ARG workspace 8 | ARG shell 9 | 10 | # Basic Utilities 11 | RUN apt-get -qq -y update && apt-get install -y zsh screen tree sudo ssh synaptic 12 | 13 | # Latest X11 / mesa GL 14 | RUN apt-get install -y\ 15 | xserver-xorg-dev-lts-wily\ 16 | libegl1-mesa-dev-lts-wily\ 17 | libgl1-mesa-dev-lts-wily\ 18 | libgbm-dev-lts-wily\ 19 | mesa-common-dev-lts-wily\ 20 | libgles2-mesa-lts-wily\ 21 | libwayland-egl1-mesa-lts-wily\ 22 | libopenvg1-mesa 23 | 24 | # Dependencies required to build rviz 25 | RUN apt-get install -y\ 26 | qt4-dev-tools\ 27 | libqt5core5a libqt5dbus5 libqt5gui5 libwayland-client0\ 28 | libwayland-server0 libxcb-icccm4 libxcb-image0 libxcb-keysyms1\ 29 | libxcb-render-util0 libxcb-util0 libxcb-xkb1 libxkbcommon-x11-0\ 30 | libxkbcommon0 31 | 32 | # The rest of ROS-desktop 33 | RUN apt-get install -y ros-indigo-desktop 34 | 35 | # Additional development tools 36 | RUN apt-get install -y x11-apps python-pip build-essential python-numpy python-scipy python-matplotlib 37 | RUN apt-get install -y nano 38 | RUN pip install catkin_tools 39 | RUN pip install pypot 40 | RUN pip install poppy-humanoid 41 | RUN pip install deap 42 | 43 | # Needed to run VREP without associating to a GUI 44 | RUN apt-get install -y xvfb 45 | 46 | # Make SSH available 47 | EXPOSE 22 48 | 49 | # Mount the user's home directory 50 | VOLUME "${home}" 51 | 52 | # Clone user into docker image and set up X11 sharing 53 | RUN \ 54 | echo "${user}:x:${uid}:${uid}:${user},,,:${home}:${shell}" >> /etc/passwd && \ 55 | echo "${user}:x:${uid}:" >> /etc/group && \ 56 | echo "${user} ALL=(ALL) NOPASSWD: ALL" > "/etc/sudoers.d/${user}" && \ 57 | chmod 0440 "/etc/sudoers.d/${user}" 58 | 59 | # Switch to user 60 | USER "${user}" 61 | # This is required for sharing Xauthority 62 | ENV QT_X11_NO_MITSHM=1 63 | ENV CATKIN_TOPLEVEL_WS="${workspace}/devel" 64 | # Switch to the workspace 65 | WORKDIR ${workspace} 66 | -------------------------------------------------------------------------------- /docker/bio_walk_paths.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Set the VREP_ROOT 4 | export VREP_ROOT=$HOME/computing/simulators/V-REP_PRO_EDU_V3_4_0_Linux 5 | # Set BIO_WALK_ROOT 6 | export BIO_WALK_ROOT=$HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking 7 | export MATSUOKA_WALK_ROOT=$HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking 8 | 9 | # Set the PYTHONPATH 10 | export PYTHONPATH=$PYTHONPATH:$BIO_WALK_ROOT/nicomotion/:$BIO_WALK_ROOT/ 11 | -------------------------------------------------------------------------------- /docker/bio_walk_setup.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # This script should be run inside the container 3 | 4 | # Set the VREP_ROOT 5 | export VREP_ROOT=$HOME/computing/simulators/V-REP_PRO_EDU_V3_4_0_Linux 6 | 7 | # Set BIO_WALK_ROOT 8 | export BIO_WALK_ROOT=$HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking 9 | 10 | # Copy VREP remote API script and start script to VREP_ROOT and set permissions 11 | cp -f $BIO_WALK_ROOT/vrep_scripts/remoteApiConnections.txt $VREP_ROOT/remoteApiConnections.txt 12 | cp -f $BIO_WALK_ROOT/vrep_scripts/start_vrep_headless.sh $VREP_ROOT/start_vrep_headless.sh 13 | chmod a+x $VREP_ROOT/start_vrep_headless.sh 14 | 15 | source /opt/ros/indigo/setup.bash 16 | mkdir -p $HOME/catkin_ws/src 17 | cd $HOME/catkin_ws/ 18 | catkin build 19 | source $HOME/catkin_ws/devel/setup.bash 20 | 21 | # Copy VREP ROS packages to catkin_ws 22 | cp -r $VREP_ROOT/programming/ros_packages/vrep_skeleton_msg_and_srv ~/catkin_ws/src/ 23 | cp -r $VREP_ROOT/programming/ros_packages/vrep_plugin_skeleton ~/catkin_ws/src/ 24 | cp -r $VREP_ROOT/programming/ros_packages/v_repExtRosInterface ~/catkin_ws/src/ 25 | cp -r $VREP_ROOT/programming/ros_packages/ros_bubble_rob2 ~/catkin_ws/src/ 26 | 27 | #Inside catkin_ws 28 | cd $HOME/catkin_ws/ 29 | catkin build 30 | source /opt/ros/indigo/setup.bash 31 | source $HOME/catkin_ws/devel/setup.bash 32 | 33 | # Copy the built libraries to VREP_ROOT 34 | cp $HOME/catkin_ws/devel/lib/*.so $VREP_ROOT 35 | 36 | # Not needed since log folder is mounted from host system 37 | # Create the log folder 38 | # mkdir -p $HOME/.bio_walk/logs 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Check args 4 | if [ "$#" -ne 1 ]; then 5 | echo "usage: ./build.sh IMAGE_NAME" 6 | return 1 7 | fi 8 | 9 | # Get this script's path 10 | pushd `dirname $0` > /dev/null 11 | SCRIPTPATH=`pwd` 12 | popd > /dev/null 13 | 14 | # Build the docker image 15 | docker build\ 16 | --no-cache\ 17 | --build-arg user=$USER\ 18 | --build-arg uid=$UID\ 19 | --build-arg home=$HOME\ 20 | --build-arg workspace=$SCRIPTPATH\ 21 | --build-arg shell=$SHELL\ 22 | -t $1 . 23 | -------------------------------------------------------------------------------- /docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Check args 4 | if [ "$#" -ne 1 ]; then 5 | echo "usage: sudo ./run.sh sayantanauddy/bio_walk:latest" 6 | return 1 7 | fi 8 | 9 | # Get this script's path 10 | pushd `dirname $0` > /dev/null 11 | SCRIPTPATH=`pwd` 12 | popd > /dev/null 13 | 14 | set -e 15 | 16 | # Run the container with shared X11 17 | docker run\ 18 | --net=host\ 19 | -e SHELL\ 20 | -e DISPLAY\ 21 | -e DOCKER=1\ 22 | -v "$HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking:/root/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking:rw"\ 23 | -v "$HOME/computing/simulators/V-REP_PRO_EDU_V3_4_0_Linux:/root/computing/simulators/V-REP_PRO_EDU_V3_4_0_Linux:rw"\ 24 | -v "$HOME/.bio_walk/logs:/root/.bio_walk/logs:rw"\ 25 | -v "/tmp/.X11-unix:/tmp/.X11-unix:rw"\ 26 | -p 80:80\ 27 | -p 19997:19997\ 28 | -p 19998:19998\ 29 | -it $1 $SHELL 30 | 31 | -------------------------------------------------------------------------------- /matsuoka_walk/__init__.py: -------------------------------------------------------------------------------- 1 | from matsuoka_walk.fitness import calc_fitness, hart6sc, rastigrin 2 | from matsuoka_walk.log import Logger, log 3 | from matsuoka_walk.oscillator import oscillator_nw 4 | from matsuoka_walk.monitor import RobotMonitorThread 5 | from matsuoka_walk.robots import Robot, Nico 6 | 7 | -------------------------------------------------------------------------------- /matsuoka_walk/cristiano2014.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | matplotlib.use('TkAgg') 4 | 5 | import matplotlib.pyplot as plt 6 | 7 | def pacemaker(kf): 8 | 9 | # Constants 10 | tau = 0.2800 11 | tau_prime = 0.4977 12 | beta = 2.5000 13 | w_0 = 2.2829 14 | u_e = 0.4111 15 | m1 = 1.0 16 | m2 = 1.0 17 | a = 1.0 18 | 19 | tau *= kf 20 | tau_prime *= kf 21 | 22 | dt = 0.01 23 | 24 | # Variables 25 | u1 = 0.0 26 | u2 = 0.0 27 | v1 = 0.0 28 | v2 = 0.0 29 | y1 = 0.0 30 | y2 = 0.0 31 | o = 0.0 32 | 33 | o_list = list() 34 | t_list = list() 35 | 36 | for t in np.arange(0.0, 5.0, dt): 37 | 38 | d_u1_dt = (-u1 - w_0*y2 -beta*v1 + u_e)/tau 39 | d_v1_dt = (-v1 + y1)/tau_prime 40 | y1 = max([0.0, u1]) 41 | 42 | d_u2_dt = (-u2 - w_0*y1 -beta*v2 + u_e)/tau 43 | d_v2_dt = (-v2 + y2)/tau_prime 44 | y2 = max([0.0, u2]) 45 | 46 | u1 += d_u1_dt * dt 47 | u2 += d_u2_dt * dt 48 | v1 += d_v1_dt * dt 49 | v2 += d_v2_dt * dt 50 | 51 | o = -m1*y1 + m2*y2 52 | 53 | o_list.append(o) 54 | t_list.append(t) 55 | 56 | return t_list, o_list 57 | 58 | # Trying out different values of kf 59 | #t_list, o_list_1 = pacemaker(0.1000) 60 | #t_list, o_list_2 = pacemaker(0.2193) # Value in paper 61 | t_list, o_list_2 = pacemaker(0.2000) 62 | t_list, o_list_3 = pacemaker(0.3000) 63 | t_list, o_list_4 = pacemaker(0.4000) 64 | 65 | font_size = 18 66 | plt.rcParams.update({'font.size': font_size}) 67 | 68 | 69 | fig, axes = plt.subplots(nrows=3, sharex=True, sharey=True) 70 | 71 | #plt.plot(t_list, o_list_1, color='blue', label='kf=0.1', linewidth=2.0) 72 | l0, = axes[0].plot(t_list, o_list_2, color='blue', label=r'$k_f=0.2$', linewidth=3.0) 73 | #axes[0].grid() 74 | axes[0].set_ylim([-0.25,0.25]) 75 | axes[0].set_xticks([1.0, 2.0, 3.0, 4.0, 5.0]) 76 | axes[0].set_yticks([-0.2, 0.0, 0.2]) 77 | for tick in axes[0].xaxis.get_major_ticks(): 78 | tick.label.set_fontsize(22) 79 | for tick in axes[0].yaxis.get_major_ticks(): 80 | tick.label.set_fontsize(22) 81 | 82 | l1, = axes[1].plot(t_list, o_list_3, color='red', label=r'$k_f=0.3$', linewidth=3.0) 83 | #axes[1].grid() 84 | axes[1].set_ylim([-0.25,0.25]) 85 | axes[1].set_xticks([1.0, 2.0, 3.0, 4.0, 5.0]) 86 | axes[1].set_yticks([-0.2, 0.0, 0.2]) 87 | for tick in axes[1].xaxis.get_major_ticks(): 88 | tick.label.set_fontsize(22) 89 | for tick in axes[1].yaxis.get_major_ticks(): 90 | tick.label.set_fontsize(22) 91 | 92 | 93 | l2, = axes[2].plot(t_list, o_list_4, color='green', label=r'$k_f=0.4$', linewidth=3.0) 94 | #axes[2].grid() 95 | axes[2].set_ylim([-0.25,0.25]) 96 | axes[2].set_xticks([1.0, 2.0, 3.0, 4.0, 5.0]) 97 | axes[2].set_yticks([-0.2, 0.0, 0.2]) 98 | for tick in axes[2].xaxis.get_major_ticks(): 99 | tick.label.set_fontsize(22) 100 | for tick in axes[2].yaxis.get_major_ticks(): 101 | tick.label.set_fontsize(22) 102 | 103 | fig.text(0.5, 0.20, 'Time (seconds)', ha='center', fontsize=25) 104 | fig.text(0.04, 0.61, 'Oscillator output (radians)', va='center', rotation='vertical', fontsize=25) 105 | 106 | fig.subplots_adjust(bottom=0.3, wspace=0.2) 107 | axes[2].legend(handles = [l0,l1,l2] , bbox_to_anchor=(1.0121, 4.0),fancybox=False, shadow=False, ncol=3) 108 | 109 | plt.show() 110 | 111 | -------------------------------------------------------------------------------- /matsuoka_walk/cristiano2014_kf_limits.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | matplotlib.use('TkAgg') 4 | 5 | import matplotlib.pyplot as plt 6 | import os 7 | 8 | # Set the home directory 9 | home_dir = os.path.expanduser('~') 10 | 11 | # Set the directory for saving plots 12 | plot_dir = os.path.join(home_dir, 'computing/repositories/hierarchical_bipedal_controller/plots') 13 | 14 | def pacemaker(kf): 15 | 16 | # Constants 17 | tau = 0.2800 18 | tau_prime = 0.4977 19 | beta = 2.5000 20 | w_0 = 2.2829 21 | u_e = 0.4111 22 | m1 = 1.0 23 | m2 = 1.0 24 | a = 1.0 25 | 26 | tau *= kf 27 | tau_prime *= kf 28 | 29 | dt = 0.01 30 | 31 | # Variables 32 | u1 = 0.0 33 | u2 = 0.0 34 | v1 = 0.0 35 | v2 = 0.0 36 | y1 = 0.0 37 | y2 = 0.0 38 | o = 0.0 39 | 40 | o_list = list() 41 | t_list = list() 42 | 43 | for t in np.arange(0.0, 10.0, dt): 44 | 45 | d_u1_dt = (-u1 - w_0*y2 -beta*v1 + u_e)/tau 46 | d_v1_dt = (-v1 + y1)/tau_prime 47 | y1 = max([0.0, u1]) 48 | 49 | d_u2_dt = (-u2 - w_0*y1 -beta*v2 + u_e)/tau 50 | d_v2_dt = (-v2 + y2)/tau_prime 51 | y2 = max([0.0, u2]) 52 | 53 | u1 += d_u1_dt * dt 54 | u2 += d_u2_dt * dt 55 | v1 += d_v1_dt * dt 56 | v2 += d_v2_dt * dt 57 | 58 | o = -m1*y1 + m2*y2 59 | 60 | o_list.append(o) 61 | t_list.append(t) 62 | 63 | return t_list, o_list 64 | 65 | # Trying out different values of kf 66 | #t_list, o_list_1 = pacemaker(0.1000) 67 | #t_list, o_list_2 = pacemaker(0.2193) # Value in paper 68 | t_list, o_list_2 = pacemaker(0.2000) 69 | t_list, o_list_3 = pacemaker(0.6) 70 | t_list, o_list_4 = pacemaker(1.0) 71 | 72 | font_size = 18 73 | plt.rcParams.update({'font.size': font_size}) 74 | 75 | 76 | fig, axes = plt.subplots(nrows=3, sharex=True, sharey=True) 77 | 78 | #plt.plot(t_list, o_list_1, color='blue', label='kf=0.1', linewidth=2.0) 79 | l0, = axes[0].plot(t_list, o_list_2, color='blue', label=r'$k_f=0.2$', linewidth=3.0) 80 | #axes[0].grid() 81 | axes[0].set_ylim([-0.25,0.25]) 82 | axes[0].set_xticks([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0]) 83 | axes[0].set_yticks([-0.2, 0.0, 0.2]) 84 | for tick in axes[0].xaxis.get_major_ticks(): 85 | tick.label.set_fontsize(22) 86 | for tick in axes[0].yaxis.get_major_ticks(): 87 | tick.label.set_fontsize(22) 88 | 89 | l1, = axes[1].plot(t_list, o_list_3, color='green', label=r'$k_f=0.6$', linewidth=3.0) 90 | #axes[1].grid() 91 | axes[1].set_ylim([-0.25,0.25]) 92 | axes[1].set_xticks([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0]) 93 | axes[1].set_yticks([-0.2, 0.0, 0.2]) 94 | for tick in axes[1].xaxis.get_major_ticks(): 95 | tick.label.set_fontsize(22) 96 | for tick in axes[1].yaxis.get_major_ticks(): 97 | tick.label.set_fontsize(22) 98 | 99 | 100 | l2, = axes[2].plot(t_list, o_list_4, color='red', label=r'$k_f=1.0$', linewidth=3.0) 101 | #axes[2].grid() 102 | axes[2].set_ylim([-0.25,0.25]) 103 | axes[2].set_xticks([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0]) 104 | axes[2].set_yticks([-0.2, 0.0, 0.2]) 105 | for tick in axes[2].xaxis.get_major_ticks(): 106 | tick.label.set_fontsize(22) 107 | for tick in axes[2].yaxis.get_major_ticks(): 108 | tick.label.set_fontsize(22) 109 | 110 | fig.text(0.5, 0.20, 'Time (seconds)', ha='center', fontsize=25) 111 | fig.text(0.04, 0.61, 'Oscillator output (radians)', va='center', rotation='vertical', fontsize=25) 112 | 113 | fig.subplots_adjust(bottom=0.3, wspace=0.2) 114 | axes[2].legend(handles = [l0,l1,l2] , bbox_to_anchor=(1.0121, 4.0),fancybox=False, shadow=False, ncol=3) 115 | 116 | plt.savefig(os.path.join(plot_dir, 'kf_limit.svg'), bbox_inches='tight') 117 | 118 | -------------------------------------------------------------------------------- /matsuoka_walk/cristiano2014_phase_reset.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | matplotlib.use('TkAgg') 4 | 5 | import matplotlib.pyplot as plt 6 | 7 | def pacemaker(kf, phase_reset_times): 8 | 9 | # Constants 10 | tau = 0.2800 11 | tau_prime = 0.4977 12 | beta = 2.5000 13 | w_0 = 2.2829 14 | u_e = 0.4111 15 | m1 = 1.0 16 | m2 = 1.0 17 | a = 1.0 18 | 19 | tau *= kf 20 | tau_prime *= kf 21 | 22 | dt = 0.01 23 | 24 | # Variables 25 | u1 = 0.0 26 | u2 = 0.0 27 | v1 = 0.0 28 | v2 = 0.0 29 | y1 = 0.0 30 | y2 = 0.0 31 | o = 0.0 32 | 33 | o_list = list() 34 | t_list = list() 35 | 36 | for t in np.arange(0.0, 5.0, dt): 37 | 38 | # Phase reset 39 | if t in phase_reset_times: 40 | u1, u2, v1, v2, y1, y2, o = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 41 | 42 | d_u1_dt = (-u1 - w_0*y2 -beta*v1 + u_e)/tau 43 | d_v1_dt = (-v1 + y1)/tau_prime 44 | y1 = max([0.0, u1]) 45 | 46 | d_u2_dt = (-u2 - w_0*y1 -beta*v2 + u_e)/tau 47 | d_v2_dt = (-v2 + y2)/tau_prime 48 | y2 = max([0.0, u2]) 49 | 50 | u1 += d_u1_dt * dt 51 | u2 += d_u2_dt * dt 52 | v1 += d_v1_dt * dt 53 | v2 += d_v2_dt * dt 54 | 55 | o = -m1*y1 + m2*y2 56 | 57 | o_list.append(o) 58 | t_list.append(t) 59 | 60 | return t_list, o_list 61 | 62 | t_list, o_list_1 = pacemaker(0.2000,phase_reset_times=[1.5,2.5,3.7]) 63 | t_list, o_list_2 = pacemaker(0.2000,phase_reset_times=[]) 64 | 65 | font_size = 18 66 | plt.rcParams.update({'font.size': font_size}) 67 | matplotlib.rc('xtick', labelsize=20) 68 | matplotlib.rc('ytick', labelsize=20) 69 | 70 | plt.figure(1,figsize=(20,2)) 71 | plt.plot(t_list, o_list_1, color='blue', ls='-', label=r'$with\ phase\ reset$', linewidth=3.0) 72 | plt.plot(t_list, o_list_2, color='blue', ls='--', label=r'$without\ phase\ reset$', linewidth=3.0) 73 | plt.plot((1.5,1.5),(-.25,.25), color='black', linewidth=2.0) 74 | plt.plot((2.5,2.5),(-.25,.25), color='black', linewidth=2.0) 75 | plt.plot((3.7,3.7),(-.25,.25), color='black', linewidth=2.0) 76 | 77 | plt.xlim(0.0,5.0) 78 | plt.ylim(-0.25,0.25) 79 | plt.xticks([0.0,1.0,1.5,2.0,2.5,3.0,3.7,4.0,5.0]) 80 | plt.yticks([-0.2,0.0,0.2]) 81 | plt.xlabel('Time (seconds)', fontsize=25) 82 | plt.ylabel('Oscillator output (radians)', fontsize=25) 83 | #plt.grid() 84 | plt.legend(bbox_to_anchor=(1.013, 1), loc='lower right', ncol=2) 85 | plt.show() 86 | -------------------------------------------------------------------------------- /matsuoka_walk/fitness.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matsuoka_walk.log import log 3 | 4 | 5 | def hart6sc(position_vector, **kwargs): 6 | """ 7 | Implementation of the Hartmann 6-dimensional function (rescaled) 8 | Adapted from Matlab implementation at https://www.sfu.ca/~ssurjano/Code/hart6scm.html 9 | Details and optimum value can be found at https://www.sfu.ca/~ssurjano/hart6.html 10 | 11 | :param position_vector: A 6D position vector 12 | :return: fitness_score 13 | """ 14 | 15 | alpha = np.array([[1.0], [1.2], [3.0], [3.2]]) 16 | A = np.array([[10.0, 3.0, 17.0, 3.50, 1.7, 8.0], 17 | [0.05, 10.0, 17.0, 0.1, 8.0, 14.0], 18 | [3.0, 3.5, 1.7, 10.0, 17.0, 8.0], 19 | [17.0, 8.0, 0.05, 10.0, 0.1, 14.0]]) 20 | 21 | P = 10**(-4) * np.array([[1312, 1696, 5569, 124, 8283, 5886], 22 | [2329, 4135, 8307, 3736, 1004, 9991], 23 | [2348, 1451, 3522, 2883, 3047, 6650], 24 | [4047, 8828, 8732, 5743, 1091, 381]]) 25 | 26 | outer = 0 27 | for ii in range(4): 28 | inner = 0 29 | for jj in range(6): 30 | xj = position_vector[jj] 31 | Aij = A[ii][jj] 32 | Pij = P[ii][jj] 33 | inner = inner + Aij * (xj - Pij)**2 34 | new = alpha[ii] * np.exp(-inner) 35 | outer = outer + new 36 | 37 | return -outer 38 | 39 | 40 | def rastigrin(position_vector): 41 | 42 | dim = len(position_vector) 43 | 44 | return 10.0*dim + sum([(xi**2 - 10.0*np.cos(2*np.pi*xi)) for xi in position_vector]) 45 | 46 | def calc_fitness(start_x, start_y, start_z, 47 | end_x, end_y, end_z, 48 | avg_z, 49 | up_time, 50 | fitness_option 51 | ): 52 | 53 | log('[FIT] Executing calc_fitness with fitness_option: {0}'.format(fitness_option)) 54 | log('[FIT] Other arguments:') 55 | log('[FIT] start_x={0}, start_y={1}, start_z={2}'.format(start_x, start_y, start_z)) 56 | log('[FIT] end_x={0}, end_y={1}, end_z={2}'.format(end_x, end_y, end_z)) 57 | log('[FIT] avg_z={0}'.format(avg_z)) 58 | log('[FIT] up_time={0}'.format(up_time)) 59 | 60 | x_distance = end_x - start_x 61 | y_distance = end_y - start_y 62 | 63 | x_vel = x_distance/up_time # (metres/second) 64 | 65 | fitness = 0.0 66 | 67 | if fitness_option==1: 68 | # Fitness is the distance moved in the x direction (metres) 69 | fitness = x_distance 70 | log('[FIT] fitness = x_distance') 71 | 72 | elif fitness_option==2: 73 | # Fitness is the distance moved in the x direction (metres) + up_time (minutes) 74 | fitness = x_distance + up_time/60.0 75 | log('[FIT] fitness = x_distance + up_time/60.0') 76 | 77 | elif fitness_option == 3: 78 | # Fitness is the distance moved in the x direction * 0.3 (metres) + up_time (seconds) * 0.7 79 | # This formula yielded the stable walk in open loop 80 | fitness = x_distance*0.3 + up_time*0.7 81 | log('[FIT] fitness = x_distance*0.3 + up_time*0.7') 82 | 83 | elif fitness_option==4: 84 | # Fitness is the straight line velocity minus a penalty for deviating from the straight line 85 | # Follows the formula in Cristiano's paper (coefficient values are aplha=80, gamma=100) 86 | fitness = 80.0*x_vel - 100*abs(y_distance) 87 | log('[FIT] fitness = 80.0*x_vel - 100*abs(y_distance)') 88 | 89 | elif fitness_option==5: 90 | # Fitness is the sum of x-distance(m), time(minutes), x-vel(m/s) 91 | fitness = x_distance + (up_time/60.0) + x_vel 92 | log('[FIT] fitness = x_distance + (up_time/60.0) + x_vel') 93 | 94 | elif fitness_option == 6: 95 | # Fitness is the distance moved in the x direction (metres) + up_time (seconds)*0.5 96 | # This formula yielded the stable walk in open loop 97 | fitness = x_distance + up_time * 0.5 98 | log('[FIT] fitness = x_distance + up_time*0.5') 99 | 100 | return fitness 101 | -------------------------------------------------------------------------------- /matsuoka_walk/ga.py: -------------------------------------------------------------------------------- 1 | import os 2 | import random 3 | 4 | from deap import base 5 | from deap import creator 6 | from deap import tools 7 | 8 | from matsuoka_walk import oscillator_nw, Logger, log 9 | 10 | # Set the home directory 11 | home_dir = os.path.expanduser('~') 12 | 13 | # Set the logging variables 14 | # This also creates a new log file 15 | Logger(log_dir=os.path.join(home_dir, '.bio_walk/logs/'), log_flag=True) 16 | 17 | # Create the position bounds of the individual 18 | log('[GA] Creating position bounds') 19 | FLT_MIN_KF, FLT_MAX_KF = 0.2, 0.5 20 | FLT_MIN_GAIN1, FLT_MAX_GAIN1 = 0.01, 1.0 21 | FLT_MIN_GAIN2, FLT_MAX_GAIN2 = 0.01, 1.0 22 | FLT_MIN_GAIN3, FLT_MAX_GAIN3 = 0.01, 1.0 23 | FLT_MIN_GAIN4, FLT_MAX_GAIN4 = 0.01, 1.0 24 | FLT_MIN_GAIN5, FLT_MAX_GAIN5 = 0.01, 1.0 25 | FLT_MIN_GAIN6, FLT_MAX_GAIN6 = 0.01, 1.0 26 | FLT_MIN_BIAS1, FLT_MAX_BIAS1 = -0.6, 0.0 27 | FLT_MIN_BIAS2, FLT_MAX_BIAS2 = 0.0, 0.5 28 | FLT_MIN_BIAS3, FLT_MAX_BIAS3 = -0.5, 0.0 29 | FLT_MIN_BIAS4, FLT_MAX_BIAS4 = 0.0, 1.0 30 | 31 | log('[GA] Logging position bounds') 32 | log('[GA] FLT_MIN_KF={0}, FLT_MAX_KF={1}'.format(FLT_MIN_KF, FLT_MAX_KF)) 33 | log('[GA] FLT_MIN_GAIN1={0}, FLT_MAX_GAIN1={1}'.format(FLT_MIN_GAIN1, FLT_MAX_GAIN1)) 34 | log('[GA] FLT_MIN_GAIN2={0}, FLT_MAX_GAIN2={1}'.format(FLT_MIN_GAIN2, FLT_MAX_GAIN2)) 35 | log('[GA] FLT_MIN_GAIN3={0}, FLT_MAX_GAIN3={1}'.format(FLT_MIN_GAIN3, FLT_MAX_GAIN3)) 36 | log('[GA] FLT_MIN_GAIN4={0}, FLT_MAX_GAIN4={1}'.format(FLT_MIN_GAIN4, FLT_MAX_GAIN4)) 37 | log('[GA] FLT_MIN_GAIN5={0}, FLT_MAX_GAIN5={1}'.format(FLT_MIN_GAIN5, FLT_MAX_GAIN5)) 38 | log('[GA] FLT_MIN_GAIN6={0}, FLT_MAX_GAIN6={1}'.format(FLT_MIN_GAIN6, FLT_MAX_GAIN6)) 39 | log('[GA] FLT_MIN_BIAS1={0}, FLT_MAX_BIAS1={1}'.format(FLT_MIN_BIAS1, FLT_MAX_BIAS1)) 40 | log('[GA] FLT_MIN_BIAS2={0}, FLT_MAX_BIAS2={1}'.format(FLT_MIN_BIAS2, FLT_MAX_BIAS2)) 41 | log('[GA] FLT_MIN_BIAS3={0}, FLT_MAX_BIAS3={1}'.format(FLT_MIN_BIAS3, FLT_MAX_BIAS3)) 42 | log('[GA] FLT_MIN_BIAS4={0}, FLT_MAX_BIAS4={1}'.format(FLT_MIN_BIAS4, FLT_MAX_BIAS4)) 43 | 44 | # Define a custom class named `FitnessMax` 45 | # Single objective function is specified by the tuple `weights=(1.0,)` 46 | creator.create("FitnessMax", base.Fitness, weights=(1.0,)) 47 | 48 | # Create a class named `Individual` which inherits from the class `list` and has `FitnessMax` as an attribute 49 | creator.create("Individual", list, fitness=creator.FitnessMax) 50 | 51 | # Now we will use our custom classes to create types representing our individuals as well as our whole population. 52 | # All the objects we will use on our way, an individual, the population, as well as all functions, operators, and 53 | # arguments will be stored in a DEAP container called `Toolbox`. It contains two methods for adding and removing 54 | # content, register() and unregister(). 55 | 56 | toolbox = base.Toolbox() 57 | 58 | # Attribute generator - specify how each single gene is to be created 59 | toolbox.register("kf_flt", random.uniform, FLT_MIN_KF, FLT_MAX_KF) 60 | toolbox.register("gain1_flt", random.uniform, FLT_MIN_GAIN1, FLT_MAX_GAIN1) 61 | toolbox.register("gain2_flt", random.uniform, FLT_MIN_GAIN2, FLT_MAX_GAIN2) 62 | toolbox.register("gain3_flt", random.uniform, FLT_MIN_GAIN3, FLT_MAX_GAIN3) 63 | toolbox.register("gain4_flt", random.uniform, FLT_MIN_GAIN4, FLT_MAX_GAIN4) 64 | toolbox.register("gain5_flt", random.uniform, FLT_MIN_GAIN5, FLT_MAX_GAIN5) 65 | toolbox.register("gain6_flt", random.uniform, FLT_MIN_GAIN6, FLT_MAX_GAIN6) 66 | toolbox.register("bias1_flt", random.uniform, FLT_MIN_BIAS1, FLT_MAX_BIAS1) 67 | toolbox.register("bias2_flt", random.uniform, FLT_MIN_BIAS2, FLT_MAX_BIAS2) 68 | toolbox.register("bias3_flt", random.uniform, FLT_MIN_BIAS3, FLT_MAX_BIAS3) 69 | toolbox.register("bias4_flt", random.uniform, FLT_MIN_BIAS4, FLT_MAX_BIAS4) 70 | 71 | # Specify the structure of an individual chromosome 72 | N_CYCLES=1 # Number of times to repeat this pattern 73 | 74 | # Specify the sequence of genes in an individual chromosome 75 | toolbox.register("individual", tools.initCycle, creator.Individual, 76 | (toolbox.kf_flt, 77 | toolbox.gain1_flt, toolbox.gain2_flt, toolbox.gain3_flt, 78 | toolbox.gain4_flt, toolbox.gain5_flt, toolbox.gain6_flt, 79 | toolbox.bias1_flt ,toolbox.bias2_flt, toolbox.bias3_flt, toolbox.bias4_flt), 80 | n=N_CYCLES) 81 | 82 | # Define the population to be a list of individuals 83 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 84 | 85 | # Register the goal / fitness function 86 | toolbox.register("evaluate", oscillator_nw) 87 | 88 | # Register the crossover operator - 2 point crossover is used here 89 | toolbox.register("mate", tools.cxTwoPoint) 90 | 91 | # Register a mutation operator 92 | # Mutation is done by adding a float to each gene. This float to be added is randomly selected from a Gaussian 93 | # distribution with mu=0.0 and sigma=0.01 94 | # Probability of mutation is 0.05 95 | toolbox.register("mutate", tools.mutGaussian, mu=0.0, sigma=0.01, indpb=0.05) 96 | 97 | # Operator for selecting individuals for breeding the next 98 | # generation: each individual of the current generation 99 | # is replaced by the 'fittest' (best) of 3 individuals 100 | # drawn randomly from the current generation. 101 | toolbox.register("select", tools.selTournament, tournsize=3) 102 | 103 | # Size of the population 104 | POP_SIZE = 200 105 | 106 | # Maximum generations 107 | MAX_GEN = 100 108 | 109 | def main(): 110 | #random.seed(64) 111 | 112 | # Create an initial population of `POP_SIZE` individuals (where each individual is a list of floats) 113 | pop = toolbox.population(n=POP_SIZE) 114 | 115 | # CXPB is the probability with which two individuals are crossed 116 | # MUTPB is the probability for mutating an individual 117 | CXPB, MUTPB = 0.8, 0.1 118 | 119 | log('[GA] Starting genetic algorithm') 120 | 121 | # Evaluate the entire population and store the fitness of each individual 122 | log('[GA] Finding the fitness of individuals in the initial generation') 123 | fitnesses = list(map(toolbox.evaluate, pop)) 124 | for ind, fit in zip(pop, fitnesses): 125 | print ind, fit 126 | ind.fitness.values = (fit,) 127 | 128 | # Extracting all the fitnesses 129 | fits = [ind.fitness.values[0] for ind in pop] 130 | 131 | # Variable keeping track of the number of generations 132 | g = 0 133 | 134 | best_ind_ever = None 135 | best_fitness_ever = 0.0 136 | 137 | # Begin the evolution 138 | while max(fits) < 100 and g < MAX_GEN: 139 | 140 | # A new generation 141 | g = g + 1 142 | log('[GA] Running generation {0}'.format(g)) 143 | 144 | # Select the next generation individuals 145 | log('[GA] Selecting the next generation') 146 | offspring = toolbox.select(pop, len(pop)) 147 | # Clone the selected individuals 148 | offspring = list(map(toolbox.clone, offspring)) 149 | 150 | # Apply crossover and mutation on the offspring 151 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 152 | # cross two individuals with probability CXPB 153 | if random.random() < CXPB: 154 | toolbox.mate(child1, child2) 155 | 156 | # fitness values of the children 157 | # must be recalculated later 158 | del child1.fitness.values 159 | del child2.fitness.values 160 | 161 | for mutant in offspring: 162 | # mutate an individual with probability MUTPB 163 | if random.random() < MUTPB: 164 | toolbox.mutate(mutant) 165 | del mutant.fitness.values 166 | 167 | # Since the content of some of our offspring changed during the last step, we now need to 168 | # re-evaluate their fitnesses. To save time and resources, we just map those offspring which 169 | # fitnesses were marked invalid. 170 | # Evaluate the individuals with an invalid fitness 171 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 172 | fitnesses = map(toolbox.evaluate, invalid_ind) 173 | for ind, fit in zip(invalid_ind, fitnesses): 174 | ind.fitness.values = (fit,) 175 | 176 | log('[GA] Evaluated {0} individuals (invalid fitness)'.format(len(invalid_ind))) 177 | 178 | # The population is entirely replaced by the offspring 179 | pop[:] = offspring 180 | 181 | # Gather all the fitnesses in one list and print the stats 182 | fits = [ind.fitness.values[0] for ind in pop] 183 | 184 | length = len(pop) 185 | mean = sum(fits) / length 186 | sum2 = sum(x * x for x in fits) 187 | std = abs(sum2 / length - mean ** 2) ** 0.5 188 | 189 | log('[GA] Results for generation {0}'.format(g)) 190 | log('[GA] Min %s' % min(fits)) 191 | log('[GA] Max %s' % max(fits)) 192 | log('[GA] Avg %s' % mean) 193 | log('[GA] Std %s' % std) 194 | 195 | best_ind_g = tools.selBest(pop, 1)[0] 196 | 197 | # Store the best individual over all generations 198 | if best_ind_g.fitness.values[0] > best_fitness_ever: 199 | best_fitness_ever = best_ind_g.fitness.values[0] 200 | best_ind_ever = best_ind_g 201 | 202 | log('[GA] Best individual for generation {0}: {1}, {2}'.format(g, best_ind_g, best_ind_g.fitness.values[0])) 203 | 204 | log('[GA] ############################# End of generation {0} #############################'.format(g)) 205 | 206 | log('[GA] ===================== End of evolution =====================') 207 | 208 | best_ind = tools.selBest(pop, 1)[0] 209 | log('[GA] Best individual in the population: %s, %s' % (best_ind, best_ind.fitness.values[0])) 210 | log('[GA] Best individual ever: %s, %s' % (best_ind_ever, best_fitness_ever)) 211 | 212 | if __name__ == "__main__": 213 | main() 214 | -------------------------------------------------------------------------------- /matsuoka_walk/ga_2.py: -------------------------------------------------------------------------------- 1 | import os 2 | import random 3 | 4 | from deap import base 5 | from deap import creator 6 | from deap import tools 7 | 8 | from matsuoka_walk import Logger, log 9 | #from matsuoka_walk.oscillator_2 import oscillator_nw 10 | # oscillator_2_1 has corrected up_time calculation 11 | from matsuoka_walk.oscillator_2_1 import oscillator_nw 12 | 13 | # Set the home directory 14 | home_dir = os.path.expanduser('~') 15 | 16 | # Set the logging variables 17 | # This also creates a new log file 18 | Logger(log_dir=os.path.join(home_dir, '.bio_walk/logs/'), log_flag=True) 19 | 20 | log('[GA] Running ga_2') 21 | 22 | # Create the position bounds of the individual 23 | log('[GA] Creating position bounds') 24 | FLT_MIN_KF, FLT_MAX_KF = 0.2, 1.0 25 | FLT_MIN_GAIN1, FLT_MAX_GAIN1 = 0.01, 1.0 26 | FLT_MIN_GAIN2, FLT_MAX_GAIN2 = 0.01, 1.0 27 | FLT_MIN_GAIN3, FLT_MAX_GAIN3 = 0.01, 1.0 28 | FLT_MIN_GAIN4, FLT_MAX_GAIN4 = 0.01, 1.0 29 | FLT_MIN_GAIN5, FLT_MAX_GAIN5 = 0.01, 1.0 30 | FLT_MIN_GAIN6, FLT_MAX_GAIN6 = 0.01, 1.0 31 | FLT_MIN_BIAS1, FLT_MAX_BIAS1 = -0.6, 0.0 32 | FLT_MIN_BIAS2, FLT_MAX_BIAS2 = 0.0, 0.5 33 | FLT_MIN_BIAS3, FLT_MAX_BIAS3 = -0.5, 0.0 34 | FLT_MIN_BIAS4, FLT_MAX_BIAS4 = 0.0, 1.0 35 | 36 | log('[GA] Logging position bounds') 37 | log('[GA] FLT_MIN_KF={0}, FLT_MAX_KF={1}'.format(FLT_MIN_KF, FLT_MAX_KF)) 38 | log('[GA] FLT_MIN_GAIN1={0}, FLT_MAX_GAIN1={1}'.format(FLT_MIN_GAIN1, FLT_MAX_GAIN1)) 39 | log('[GA] FLT_MIN_GAIN2={0}, FLT_MAX_GAIN2={1}'.format(FLT_MIN_GAIN2, FLT_MAX_GAIN2)) 40 | log('[GA] FLT_MIN_GAIN3={0}, FLT_MAX_GAIN3={1}'.format(FLT_MIN_GAIN3, FLT_MAX_GAIN3)) 41 | log('[GA] FLT_MIN_GAIN4={0}, FLT_MAX_GAIN4={1}'.format(FLT_MIN_GAIN4, FLT_MAX_GAIN4)) 42 | log('[GA] FLT_MIN_GAIN5={0}, FLT_MAX_GAIN5={1}'.format(FLT_MIN_GAIN5, FLT_MAX_GAIN5)) 43 | log('[GA] FLT_MIN_GAIN6={0}, FLT_MAX_GAIN6={1}'.format(FLT_MIN_GAIN6, FLT_MAX_GAIN6)) 44 | log('[GA] FLT_MIN_BIAS1={0}, FLT_MAX_BIAS1={1}'.format(FLT_MIN_BIAS1, FLT_MAX_BIAS1)) 45 | log('[GA] FLT_MIN_BIAS2={0}, FLT_MAX_BIAS2={1}'.format(FLT_MIN_BIAS2, FLT_MAX_BIAS2)) 46 | log('[GA] FLT_MIN_BIAS3={0}, FLT_MAX_BIAS3={1}'.format(FLT_MIN_BIAS3, FLT_MAX_BIAS3)) 47 | log('[GA] FLT_MIN_BIAS4={0}, FLT_MAX_BIAS4={1}'.format(FLT_MIN_BIAS4, FLT_MAX_BIAS4)) 48 | 49 | # Define a custom class named `FitnessMax` 50 | # Single objective function is specified by the tuple `weights=(1.0,)` 51 | creator.create("FitnessMax", base.Fitness, weights=(1.0,)) 52 | 53 | # Create a class named `Individual` which inherits from the class `list` and has `FitnessMax` as an attribute 54 | creator.create("Individual", list, fitness=creator.FitnessMax) 55 | 56 | # Now we will use our custom classes to create types representing our individuals as well as our whole population. 57 | # All the objects we will use on our way, an individual, the population, as well as all functions, operators, and 58 | # arguments will be stored in a DEAP container called `Toolbox`. It contains two methods for adding and removing 59 | # content, register() and unregister(). 60 | 61 | toolbox = base.Toolbox() 62 | 63 | # Attribute generator - specify how each single gene is to be created 64 | toolbox.register("kf_flt", random.uniform, FLT_MIN_KF, FLT_MAX_KF) 65 | toolbox.register("gain1_flt", random.uniform, FLT_MIN_GAIN1, FLT_MAX_GAIN1) 66 | toolbox.register("gain2_flt", random.uniform, FLT_MIN_GAIN2, FLT_MAX_GAIN2) 67 | toolbox.register("gain3_flt", random.uniform, FLT_MIN_GAIN3, FLT_MAX_GAIN3) 68 | toolbox.register("gain4_flt", random.uniform, FLT_MIN_GAIN4, FLT_MAX_GAIN4) 69 | toolbox.register("gain5_flt", random.uniform, FLT_MIN_GAIN5, FLT_MAX_GAIN5) 70 | toolbox.register("gain6_flt", random.uniform, FLT_MIN_GAIN6, FLT_MAX_GAIN6) 71 | toolbox.register("bias1_flt", random.uniform, FLT_MIN_BIAS1, FLT_MAX_BIAS1) 72 | toolbox.register("bias2_flt", random.uniform, FLT_MIN_BIAS2, FLT_MAX_BIAS2) 73 | toolbox.register("bias3_flt", random.uniform, FLT_MIN_BIAS3, FLT_MAX_BIAS3) 74 | toolbox.register("bias4_flt", random.uniform, FLT_MIN_BIAS4, FLT_MAX_BIAS4) 75 | 76 | # Specify the structure of an individual chromosome 77 | N_CYCLES=1 # Number of times to repeat this pattern 78 | 79 | # Specify the sequence of genes in an individual chromosome 80 | toolbox.register("individual", tools.initCycle, creator.Individual, 81 | (toolbox.kf_flt, 82 | toolbox.gain1_flt, toolbox.gain2_flt, toolbox.gain3_flt, 83 | toolbox.gain4_flt, toolbox.gain5_flt, toolbox.gain6_flt, 84 | toolbox.bias1_flt ,toolbox.bias2_flt, toolbox.bias3_flt, toolbox.bias4_flt), 85 | n=N_CYCLES) 86 | 87 | # Define the population to be a list of individuals 88 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 89 | 90 | # Register the goal / fitness function 91 | toolbox.register("evaluate", oscillator_nw) 92 | 93 | # Register the crossover operator - 2 point crossover is used here 94 | toolbox.register("mate", tools.cxTwoPoint) 95 | 96 | # Register a mutation operator 97 | # Mutation is done by adding a float to each gene. This float to be added is randomly selected from a Gaussian 98 | # distribution with mu=0.0 and sigma=0.01 99 | # Probability of mutation is 0.05 100 | toolbox.register("mutate", tools.mutGaussian, mu=0.0, sigma=0.01, indpb=0.05) 101 | 102 | # Operator for selecting individuals for breeding the next 103 | # generation: each individual of the current generation 104 | # is replaced by the 'fittest' (best) of 3 individuals 105 | # drawn randomly from the current generation. 106 | toolbox.register("select", tools.selTournament, tournsize=3) 107 | 108 | # Size of the population 109 | POP_SIZE = 200 110 | 111 | # Maximum generations 112 | MAX_GEN = 31 113 | 114 | def main(): 115 | #random.seed(64) 116 | 117 | # Create an initial population of `POP_SIZE` individuals (where each individual is a list of floats) 118 | pop = toolbox.population(n=POP_SIZE) 119 | 120 | # CXPB is the probability with which two individuals are crossed 121 | # MUTPB is the probability for mutating an individual 122 | CXPB, MUTPB = 0.8, 0.1 123 | 124 | log('[GA] Starting genetic algorithm') 125 | 126 | # Evaluate the entire population and store the fitness of each individual 127 | log('[GA] Finding the fitness of individuals in the initial generation') 128 | fitnesses = list(map(toolbox.evaluate, pop)) 129 | for ind, fit in zip(pop, fitnesses): 130 | print ind, fit 131 | ind.fitness.values = (fit,) 132 | 133 | # Extracting all the fitnesses 134 | fits = [ind.fitness.values[0] for ind in pop] 135 | 136 | # Variable keeping track of the number of generations 137 | g = 0 138 | 139 | best_ind_ever = None 140 | best_fitness_ever = 0.0 141 | 142 | # Begin the evolution 143 | while max(fits) < 100 and g < MAX_GEN: 144 | 145 | # A new generation 146 | g = g + 1 147 | log('[GA] Running generation {0}'.format(g)) 148 | 149 | # Select the next generation individuals 150 | log('[GA] Selecting the next generation') 151 | offspring = toolbox.select(pop, len(pop)) 152 | # Clone the selected individuals 153 | offspring = list(map(toolbox.clone, offspring)) 154 | 155 | # Apply crossover and mutation on the offspring 156 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 157 | # cross two individuals with probability CXPB 158 | if random.random() < CXPB: 159 | toolbox.mate(child1, child2) 160 | 161 | # fitness values of the children 162 | # must be recalculated later 163 | del child1.fitness.values 164 | del child2.fitness.values 165 | 166 | for mutant in offspring: 167 | # mutate an individual with probability MUTPB 168 | if random.random() < MUTPB: 169 | toolbox.mutate(mutant) 170 | del mutant.fitness.values 171 | 172 | # Since the content of some of our offspring changed during the last step, we now need to 173 | # re-evaluate their fitnesses. To save time and resources, we just map those offspring which 174 | # fitnesses were marked invalid. 175 | # Evaluate the individuals with an invalid fitness 176 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 177 | fitnesses = map(toolbox.evaluate, invalid_ind) 178 | for ind, fit in zip(invalid_ind, fitnesses): 179 | ind.fitness.values = (fit,) 180 | 181 | log('[GA] Evaluated {0} individuals (invalid fitness)'.format(len(invalid_ind))) 182 | 183 | # The population is entirely replaced by the offspring 184 | pop[:] = offspring 185 | 186 | # Gather all the fitnesses in one list and print the stats 187 | fits = [ind.fitness.values[0] for ind in pop] 188 | 189 | length = len(pop) 190 | mean = sum(fits) / length 191 | sum2 = sum(x * x for x in fits) 192 | std = abs(sum2 / length - mean ** 2) ** 0.5 193 | 194 | log('[GA] Results for generation {0}'.format(g)) 195 | log('[GA] Min %s' % min(fits)) 196 | log('[GA] Max %s' % max(fits)) 197 | log('[GA] Avg %s' % mean) 198 | log('[GA] Std %s' % std) 199 | 200 | best_ind_g = tools.selBest(pop, 1)[0] 201 | 202 | # Store the best individual over all generations 203 | if best_ind_g.fitness.values[0] > best_fitness_ever: 204 | best_fitness_ever = best_ind_g.fitness.values[0] 205 | best_ind_ever = best_ind_g 206 | 207 | log('[GA] Best individual for generation {0}: {1}, {2}'.format(g, best_ind_g, best_ind_g.fitness.values[0])) 208 | log('[GA] Best individual ever till now: %s, %s' % (best_ind_ever, best_fitness_ever)) 209 | 210 | log('[GA] ############################# End of generation {0} #############################'.format(g)) 211 | 212 | log('[GA] ===================== End of evolution =====================') 213 | 214 | best_ind = tools.selBest(pop, 1)[0] 215 | log('[GA] Best individual in the population: %s, %s' % (best_ind, best_ind.fitness.values[0])) 216 | log('[GA] Best individual ever: %s, %s' % (best_ind_ever, best_fitness_ever)) 217 | 218 | if __name__ == "__main__": 219 | main() 220 | -------------------------------------------------------------------------------- /matsuoka_walk/ga_3.py: -------------------------------------------------------------------------------- 1 | import os 2 | import random 3 | 4 | from deap import base 5 | from deap import creator 6 | from deap import tools 7 | 8 | from matsuoka_walk import Logger, log 9 | from matsuoka_walk.oscillator_3 import oscillator_nw 10 | 11 | # Set the home directory 12 | home_dir = os.path.expanduser('~') 13 | 14 | # Set the logging variables 15 | # This also creates a new log file 16 | Logger(log_dir=os.path.join(home_dir, '.bio_walk/logs/'), log_flag=True) 17 | 18 | log('[GA] Running ga_3') 19 | 20 | # Create the position bounds of the individual 21 | log('[GA] Creating position bounds') 22 | FLT_MIN_KF, FLT_MAX_KF = 0.2, 1.0 23 | FLT_MIN_GAIN1, FLT_MAX_GAIN1 = 0.01, 1.0 24 | FLT_MIN_GAIN2, FLT_MAX_GAIN2 = 0.01, 1.0 25 | FLT_MIN_GAIN3, FLT_MAX_GAIN3 = 0.01, 1.0 26 | FLT_MIN_GAIN4, FLT_MAX_GAIN4 = 0.01, 1.0 27 | FLT_MIN_GAIN5, FLT_MAX_GAIN5 = 0.01, 1.0 28 | FLT_MIN_GAIN6, FLT_MAX_GAIN6 = 0.01, 1.0 29 | FLT_MIN_BIAS1, FLT_MAX_BIAS1 = -0.6, 0.0 30 | FLT_MIN_BIAS2, FLT_MAX_BIAS2 = 0.0, 0.5 31 | FLT_MIN_BIAS3, FLT_MAX_BIAS3 = -0.5, 0.0 32 | FLT_MIN_BIAS4, FLT_MAX_BIAS4 = 0.0, 1.0 33 | #FLT_MIN_K, FLT_MAX_K = 0.01, 1.0 34 | FLT_MIN_K, FLT_MAX_K = -2.5, 2.5 35 | 36 | log('[GA] Logging position bounds') 37 | log('[GA] FLT_MIN_KF={0}, FLT_MAX_KF={1}'.format(FLT_MIN_KF, FLT_MAX_KF)) 38 | log('[GA] FLT_MIN_GAIN1={0}, FLT_MAX_GAIN1={1}'.format(FLT_MIN_GAIN1, FLT_MAX_GAIN1)) 39 | log('[GA] FLT_MIN_GAIN2={0}, FLT_MAX_GAIN2={1}'.format(FLT_MIN_GAIN2, FLT_MAX_GAIN2)) 40 | log('[GA] FLT_MIN_GAIN3={0}, FLT_MAX_GAIN3={1}'.format(FLT_MIN_GAIN3, FLT_MAX_GAIN3)) 41 | log('[GA] FLT_MIN_GAIN4={0}, FLT_MAX_GAIN4={1}'.format(FLT_MIN_GAIN4, FLT_MAX_GAIN4)) 42 | log('[GA] FLT_MIN_GAIN5={0}, FLT_MAX_GAIN5={1}'.format(FLT_MIN_GAIN5, FLT_MAX_GAIN5)) 43 | log('[GA] FLT_MIN_GAIN6={0}, FLT_MAX_GAIN6={1}'.format(FLT_MIN_GAIN6, FLT_MAX_GAIN6)) 44 | log('[GA] FLT_MIN_BIAS1={0}, FLT_MAX_BIAS1={1}'.format(FLT_MIN_BIAS1, FLT_MAX_BIAS1)) 45 | log('[GA] FLT_MIN_BIAS2={0}, FLT_MAX_BIAS2={1}'.format(FLT_MIN_BIAS2, FLT_MAX_BIAS2)) 46 | log('[GA] FLT_MIN_BIAS3={0}, FLT_MAX_BIAS3={1}'.format(FLT_MIN_BIAS3, FLT_MAX_BIAS3)) 47 | log('[GA] FLT_MIN_BIAS4={0}, FLT_MAX_BIAS4={1}'.format(FLT_MIN_BIAS4, FLT_MAX_BIAS4)) 48 | log('[GA] FLT_MIN_K={0}, FLT_MAX_K={1}'.format(FLT_MIN_K, FLT_MAX_K)) 49 | 50 | # Define a custom class named `FitnessMax` 51 | # Single objective function is specified by the tuple `weights=(1.0,)` 52 | creator.create("FitnessMax", base.Fitness, weights=(1.0,)) 53 | 54 | # Create a class named `Individual` which inherits from the class `list` and has `FitnessMax` as an attribute 55 | creator.create("Individual", list, fitness=creator.FitnessMax) 56 | 57 | # Now we will use our custom classes to create types representing our individuals as well as our whole population. 58 | # All the objects we will use on our way, an individual, the population, as well as all functions, operators, and 59 | # arguments will be stored in a DEAP container called `Toolbox`. It contains two methods for adding and removing 60 | # content, register() and unregister(). 61 | 62 | toolbox = base.Toolbox() 63 | 64 | # Attribute generator - specify how each single gene is to be created 65 | toolbox.register("kf_flt", random.uniform, FLT_MIN_KF, FLT_MAX_KF) 66 | toolbox.register("gain1_flt", random.uniform, FLT_MIN_GAIN1, FLT_MAX_GAIN1) 67 | toolbox.register("gain2_flt", random.uniform, FLT_MIN_GAIN2, FLT_MAX_GAIN2) 68 | toolbox.register("gain3_flt", random.uniform, FLT_MIN_GAIN3, FLT_MAX_GAIN3) 69 | toolbox.register("gain4_flt", random.uniform, FLT_MIN_GAIN4, FLT_MAX_GAIN4) 70 | toolbox.register("gain5_flt", random.uniform, FLT_MIN_GAIN5, FLT_MAX_GAIN5) 71 | toolbox.register("gain6_flt", random.uniform, FLT_MIN_GAIN6, FLT_MAX_GAIN6) 72 | toolbox.register("bias1_flt", random.uniform, FLT_MIN_BIAS1, FLT_MAX_BIAS1) 73 | toolbox.register("bias2_flt", random.uniform, FLT_MIN_BIAS2, FLT_MAX_BIAS2) 74 | toolbox.register("bias3_flt", random.uniform, FLT_MIN_BIAS3, FLT_MAX_BIAS3) 75 | toolbox.register("bias4_flt", random.uniform, FLT_MIN_BIAS4, FLT_MAX_BIAS4) 76 | toolbox.register("k_flt", random.uniform, FLT_MIN_K, FLT_MAX_K) 77 | 78 | # Specify the structure of an individual chromosome 79 | N_CYCLES=1 # Number of times to repeat this pattern 80 | 81 | # Specify the sequence of genes in an individual chromosome 82 | toolbox.register("individual", tools.initCycle, creator.Individual, 83 | (toolbox.kf_flt, 84 | toolbox.gain1_flt, toolbox.gain2_flt, toolbox.gain3_flt, 85 | toolbox.gain4_flt, toolbox.gain5_flt, toolbox.gain6_flt, 86 | toolbox.bias1_flt ,toolbox.bias2_flt, toolbox.bias3_flt, toolbox.bias4_flt, 87 | toolbox.k_flt), 88 | n=N_CYCLES) 89 | 90 | # Define the population to be a list of individuals 91 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 92 | 93 | # Register the goal / fitness function 94 | toolbox.register("evaluate", oscillator_nw) 95 | 96 | # Register the crossover operator - 2 point crossover is used here 97 | toolbox.register("mate", tools.cxTwoPoint) 98 | 99 | # Register a mutation operator 100 | # Mutation is done by adding a float to each gene. This float to be added is randomly selected from a Gaussian 101 | # distribution with mu=0.0 and sigma=0.01 102 | # Probability of mutation is 0.05 103 | toolbox.register("mutate", tools.mutGaussian, mu=0.0, sigma=0.01, indpb=0.05) 104 | 105 | # Operator for selecting individuals for breeding the next 106 | # generation: each individual of the current generation 107 | # is replaced by the 'fittest' (best) of 3 individuals 108 | # drawn randomly from the current generation. 109 | toolbox.register("select", tools.selTournament, tournsize=3) 110 | 111 | # Size of the population 112 | POP_SIZE = 200 113 | 114 | # Maximum generations 115 | MAX_GEN = 32 116 | 117 | def main(): 118 | #random.seed(64) 119 | 120 | # Create an initial population of `POP_SIZE` individuals (where each individual is a list of floats) 121 | pop = toolbox.population(n=POP_SIZE) 122 | 123 | # CXPB is the probability with which two individuals are crossed 124 | # MUTPB is the probability for mutating an individual 125 | CXPB, MUTPB = 0.8, 0.1 126 | 127 | log('[GA] Starting genetic algorithm') 128 | 129 | # Evaluate the entire population and store the fitness of each individual 130 | log('[GA] Finding the fitness of individuals in the initial generation') 131 | fitnesses = list(map(toolbox.evaluate, pop)) 132 | for ind, fit in zip(pop, fitnesses): 133 | print ind, fit 134 | ind.fitness.values = (fit,) 135 | 136 | # Extracting all the fitnesses 137 | fits = [ind.fitness.values[0] for ind in pop] 138 | 139 | # Variable keeping track of the number of generations 140 | g = 0 141 | 142 | best_ind_ever = None 143 | best_fitness_ever = 0.0 144 | 145 | # Begin the evolution 146 | while max(fits) < 100 and g < MAX_GEN: 147 | 148 | # A new generation 149 | g = g + 1 150 | log('[GA] Running generation {0}'.format(g)) 151 | 152 | # Select the next generation individuals 153 | log('[GA] Selecting the next generation') 154 | offspring = toolbox.select(pop, len(pop)) 155 | # Clone the selected individuals 156 | offspring = list(map(toolbox.clone, offspring)) 157 | 158 | # Apply crossover and mutation on the offspring 159 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 160 | # cross two individuals with probability CXPB 161 | if random.random() < CXPB: 162 | toolbox.mate(child1, child2) 163 | 164 | # fitness values of the children 165 | # must be recalculated later 166 | del child1.fitness.values 167 | del child2.fitness.values 168 | 169 | for mutant in offspring: 170 | # mutate an individual with probability MUTPB 171 | if random.random() < MUTPB: 172 | toolbox.mutate(mutant) 173 | del mutant.fitness.values 174 | 175 | # Since the content of some of our offspring changed during the last step, we now need to 176 | # re-evaluate their fitnesses. To save time and resources, we just map those offspring which 177 | # fitnesses were marked invalid. 178 | # Evaluate the individuals with an invalid fitness 179 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 180 | fitnesses = map(toolbox.evaluate, invalid_ind) 181 | for ind, fit in zip(invalid_ind, fitnesses): 182 | ind.fitness.values = (fit,) 183 | 184 | log('[GA] Evaluated {0} individuals (invalid fitness)'.format(len(invalid_ind))) 185 | 186 | # The population is entirely replaced by the offspring 187 | pop[:] = offspring 188 | 189 | # Gather all the fitnesses in one list and print the stats 190 | fits = [ind.fitness.values[0] for ind in pop] 191 | 192 | length = len(pop) 193 | mean = sum(fits) / length 194 | sum2 = sum(x * x for x in fits) 195 | std = abs(sum2 / length - mean ** 2) ** 0.5 196 | 197 | log('[GA] Results for generation {0}'.format(g)) 198 | log('[GA] Min %s' % min(fits)) 199 | log('[GA] Max %s' % max(fits)) 200 | log('[GA] Avg %s' % mean) 201 | log('[GA] Std %s' % std) 202 | 203 | best_ind_g = tools.selBest(pop, 1)[0] 204 | 205 | # Store the best individual over all generations 206 | if best_ind_g.fitness.values[0] > best_fitness_ever: 207 | best_fitness_ever = best_ind_g.fitness.values[0] 208 | best_ind_ever = best_ind_g 209 | 210 | log('[GA] Best individual for generation {0}: {1}, {2}'.format(g, best_ind_g, best_ind_g.fitness.values[0])) 211 | log('[GA] Best individual ever till now: %s, %s' % (best_ind_ever, best_fitness_ever)) 212 | 213 | log('[GA] ############################# End of generation {0} #############################'.format(g)) 214 | 215 | log('[GA] ===================== End of evolution =====================') 216 | 217 | best_ind = tools.selBest(pop, 1)[0] 218 | log('[GA] Best individual in the population: %s, %s' % (best_ind, best_ind.fitness.values[0])) 219 | log('[GA] Best individual ever: %s, %s' % (best_ind_ever, best_fitness_ever)) 220 | 221 | if __name__ == "__main__": 222 | main() 223 | -------------------------------------------------------------------------------- /matsuoka_walk/ga_4.py: -------------------------------------------------------------------------------- 1 | # Script with low level feedback for all angles 2 | 3 | import os 4 | import random 5 | 6 | from deap import base 7 | from deap import creator 8 | from deap import tools 9 | 10 | from matsuoka_walk import Logger, log 11 | from matsuoka_walk.oscillator_4 import oscillator_nw 12 | 13 | # Set the home directory 14 | home_dir = os.path.expanduser('~') 15 | 16 | # Set the logging variables 17 | # This also creates a new log file 18 | Logger(log_dir=os.path.join(home_dir, '.bio_walk/logs/'), log_flag=True) 19 | 20 | log('[GA] Running ga_4') 21 | 22 | # Create the position bounds of the individual 23 | log('[GA] Creating position bounds') 24 | FLT_MIN_KF, FLT_MAX_KF = 0.2, 1.0 25 | FLT_MIN_GAIN1, FLT_MAX_GAIN1 = 0.01, 1.0 26 | FLT_MIN_GAIN2, FLT_MAX_GAIN2 = 0.01, 1.0 27 | FLT_MIN_GAIN3, FLT_MAX_GAIN3 = 0.01, 1.0 28 | FLT_MIN_GAIN4, FLT_MAX_GAIN4 = 0.01, 1.0 29 | FLT_MIN_GAIN5, FLT_MAX_GAIN5 = 0.01, 1.0 30 | FLT_MIN_GAIN6, FLT_MAX_GAIN6 = 0.01, 1.0 31 | FLT_MIN_BIAS1, FLT_MAX_BIAS1 = -0.6, 0.0 32 | FLT_MIN_BIAS2, FLT_MAX_BIAS2 = 0.0, 0.5 33 | FLT_MIN_BIAS3, FLT_MAX_BIAS3 = -0.5, 0.0 34 | FLT_MIN_BIAS4, FLT_MAX_BIAS4 = 0.0, 1.0 35 | FLT_MIN_K_HIP_Y, FLT_MAX_K_HIP_Y = -2.5, 2.5 36 | FLT_MIN_K_KNEE_Y, FLT_MAX_K_KNEE_Y = -2.5, 2.5 37 | FLT_MIN_K_ANKLE_Y, FLT_MAX_K_ANKLE_Y = -2.5, 2.5 38 | FLT_MIN_K_HIP_X, FLT_MAX_K_HIP_X = -2.5, 2.5 39 | FLT_MIN_K_ANKLE_X, FLT_MAX_K_ANKLE_X = -2.5, 2.5 40 | FLT_MIN_K_SHOULDER_Y, FLT_MAX_K_SHOULDER_Y = -2.5, 2.5 41 | 42 | log('[GA] Logging position bounds') 43 | log('[GA] FLT_MIN_KF={0}, FLT_MAX_KF={1}'.format(FLT_MIN_KF, FLT_MAX_KF)) 44 | log('[GA] FLT_MIN_GAIN1={0}, FLT_MAX_GAIN1={1}'.format(FLT_MIN_GAIN1, FLT_MAX_GAIN1)) 45 | log('[GA] FLT_MIN_GAIN2={0}, FLT_MAX_GAIN2={1}'.format(FLT_MIN_GAIN2, FLT_MAX_GAIN2)) 46 | log('[GA] FLT_MIN_GAIN3={0}, FLT_MAX_GAIN3={1}'.format(FLT_MIN_GAIN3, FLT_MAX_GAIN3)) 47 | log('[GA] FLT_MIN_GAIN4={0}, FLT_MAX_GAIN4={1}'.format(FLT_MIN_GAIN4, FLT_MAX_GAIN4)) 48 | log('[GA] FLT_MIN_GAIN5={0}, FLT_MAX_GAIN5={1}'.format(FLT_MIN_GAIN5, FLT_MAX_GAIN5)) 49 | log('[GA] FLT_MIN_GAIN6={0}, FLT_MAX_GAIN6={1}'.format(FLT_MIN_GAIN6, FLT_MAX_GAIN6)) 50 | log('[GA] FLT_MIN_BIAS1={0}, FLT_MAX_BIAS1={1}'.format(FLT_MIN_BIAS1, FLT_MAX_BIAS1)) 51 | log('[GA] FLT_MIN_BIAS2={0}, FLT_MAX_BIAS2={1}'.format(FLT_MIN_BIAS2, FLT_MAX_BIAS2)) 52 | log('[GA] FLT_MIN_BIAS3={0}, FLT_MAX_BIAS3={1}'.format(FLT_MIN_BIAS3, FLT_MAX_BIAS3)) 53 | log('[GA] FLT_MIN_BIAS4={0}, FLT_MAX_BIAS4={1}'.format(FLT_MIN_BIAS4, FLT_MAX_BIAS4)) 54 | log('[GA] FLT_MIN_K_HIP_Y={0}, FLT_MAX_K_HIP_Y={1}'.format(FLT_MIN_K_HIP_Y, FLT_MAX_K_HIP_Y)) 55 | log('[GA] FLT_MIN_K_KNEE_Y={0}, FLT_MAX_K_KNEE_Y={1}'.format(FLT_MIN_K_KNEE_Y, FLT_MAX_K_KNEE_Y)) 56 | log('[GA] FLT_MIN_K_ANKLE_Y={0}, FLT_MAX_K_ANKLE_Y={1}'.format(FLT_MIN_K_ANKLE_Y, FLT_MAX_K_ANKLE_Y)) 57 | log('[GA] FLT_MIN_K_HIP_X={0}, FLT_MAX_K_HIP_X={1}'.format(FLT_MIN_K_HIP_X, FLT_MAX_K_HIP_X)) 58 | log('[GA] FLT_MIN_K_ANKLE_X={0}, FLT_MAX_K_ANKLE_X={1}'.format(FLT_MIN_K_ANKLE_X, FLT_MAX_K_ANKLE_X)) 59 | log('[GA] FLT_MIN_K_SHOULDER_Y={0}, FLT_MAX_K_SHOULDER_Y={1}'.format(FLT_MIN_K_SHOULDER_Y, FLT_MAX_K_SHOULDER_Y)) 60 | 61 | # Define a custom class named `FitnessMax` 62 | # Single objective function is specified by the tuple `weights=(1.0,)` 63 | creator.create("FitnessMax", base.Fitness, weights=(1.0,)) 64 | 65 | # Create a class named `Individual` which inherits from the class `list` and has `FitnessMax` as an attribute 66 | creator.create("Individual", list, fitness=creator.FitnessMax) 67 | 68 | # Now we will use our custom classes to create types representing our individuals as well as our whole population. 69 | # All the objects we will use on our way, an individual, the population, as well as all functions, operators, and 70 | # arguments will be stored in a DEAP container called `Toolbox`. It contains two methods for adding and removing 71 | # content, register() and unregister(). 72 | 73 | toolbox = base.Toolbox() 74 | 75 | # Attribute generator - specify how each single gene is to be created 76 | toolbox.register("kf_flt", random.uniform, FLT_MIN_KF, FLT_MAX_KF) 77 | toolbox.register("gain1_flt", random.uniform, FLT_MIN_GAIN1, FLT_MAX_GAIN1) 78 | toolbox.register("gain2_flt", random.uniform, FLT_MIN_GAIN2, FLT_MAX_GAIN2) 79 | toolbox.register("gain3_flt", random.uniform, FLT_MIN_GAIN3, FLT_MAX_GAIN3) 80 | toolbox.register("gain4_flt", random.uniform, FLT_MIN_GAIN4, FLT_MAX_GAIN4) 81 | toolbox.register("gain5_flt", random.uniform, FLT_MIN_GAIN5, FLT_MAX_GAIN5) 82 | toolbox.register("gain6_flt", random.uniform, FLT_MIN_GAIN6, FLT_MAX_GAIN6) 83 | toolbox.register("bias1_flt", random.uniform, FLT_MIN_BIAS1, FLT_MAX_BIAS1) 84 | toolbox.register("bias2_flt", random.uniform, FLT_MIN_BIAS2, FLT_MAX_BIAS2) 85 | toolbox.register("bias3_flt", random.uniform, FLT_MIN_BIAS3, FLT_MAX_BIAS3) 86 | toolbox.register("bias4_flt", random.uniform, FLT_MIN_BIAS4, FLT_MAX_BIAS4) 87 | toolbox.register("k_hip_y_flt", random.uniform, FLT_MIN_K_HIP_Y, FLT_MAX_K_HIP_Y) 88 | toolbox.register("k_knee_y_flt", random.uniform, FLT_MIN_K_KNEE_Y, FLT_MAX_K_KNEE_Y) 89 | toolbox.register("k_ankle_y_flt", random.uniform, FLT_MIN_K_ANKLE_Y, FLT_MAX_K_ANKLE_Y) 90 | toolbox.register("k_hip_x_flt", random.uniform, FLT_MIN_K_HIP_X, FLT_MAX_K_HIP_X) 91 | toolbox.register("k_ankle_x_flt", random.uniform, FLT_MIN_K_ANKLE_X, FLT_MAX_K_ANKLE_X) 92 | toolbox.register("k_shoulder_y_flt", random.uniform, FLT_MIN_K_SHOULDER_Y, FLT_MAX_K_SHOULDER_Y) 93 | 94 | # Specify the structure of an individual chromosome 95 | N_CYCLES=1 # Number of times to repeat this pattern 96 | 97 | # Specify the sequence of genes in an individual chromosome 98 | toolbox.register("individual", tools.initCycle, creator.Individual, 99 | (toolbox.kf_flt, 100 | toolbox.gain1_flt, toolbox.gain2_flt, toolbox.gain3_flt, 101 | toolbox.gain4_flt, toolbox.gain5_flt, toolbox.gain6_flt, 102 | toolbox.bias1_flt ,toolbox.bias2_flt, toolbox.bias3_flt, toolbox.bias4_flt, 103 | toolbox.k_hip_y_flt, toolbox.k_knee_y_flt, toolbox.k_ankle_y_flt, 104 | toolbox.k_hip_x_flt, toolbox.k_ankle_x_flt, 105 | toolbox.k_shoulder_y_flt), 106 | n=N_CYCLES) 107 | 108 | # Define the population to be a list of individuals 109 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 110 | 111 | # Register the goal / fitness function 112 | toolbox.register("evaluate", oscillator_nw) 113 | 114 | # Register the crossover operator - 2 point crossover is used here 115 | toolbox.register("mate", tools.cxTwoPoint) 116 | 117 | # Register a mutation operator 118 | # Mutation is done by adding a float to each gene. This float to be added is randomly selected from a Gaussian 119 | # distribution with mu=0.0 and sigma=0.01 120 | # Probability of mutation is 0.05 121 | toolbox.register("mutate", tools.mutGaussian, mu=0.0, sigma=0.01, indpb=0.05) 122 | 123 | # Operator for selecting individuals for breeding the next 124 | # generation: each individual of the current generation 125 | # is replaced by the 'fittest' (best) of 3 individuals 126 | # drawn randomly from the current generation. 127 | toolbox.register("select", tools.selTournament, tournsize=3) 128 | 129 | # Size of the population 130 | POP_SIZE = 200 131 | 132 | # Maximum generations 133 | MAX_GEN = 100 134 | 135 | def main(): 136 | #random.seed(64) 137 | 138 | # Create an initial population of `POP_SIZE` individuals (where each individual is a list of floats) 139 | pop = toolbox.population(n=POP_SIZE) 140 | 141 | # CXPB is the probability with which two individuals are crossed 142 | # MUTPB is the probability for mutating an individual 143 | CXPB, MUTPB = 0.8, 0.1 144 | 145 | log('[GA] Starting genetic algorithm') 146 | 147 | # Evaluate the entire population and store the fitness of each individual 148 | log('[GA] Finding the fitness of individuals in the initial generation') 149 | fitnesses = list(map(toolbox.evaluate, pop)) 150 | for ind, fit in zip(pop, fitnesses): 151 | print ind, fit 152 | ind.fitness.values = (fit,) 153 | 154 | # Extracting all the fitnesses 155 | fits = [ind.fitness.values[0] for ind in pop] 156 | 157 | # Variable keeping track of the number of generations 158 | g = 0 159 | 160 | best_ind_ever = None 161 | best_fitness_ever = 0.0 162 | 163 | # Begin the evolution 164 | while max(fits) < 100 and g < MAX_GEN: 165 | 166 | # A new generation 167 | g = g + 1 168 | log('[GA] Running generation {0}'.format(g)) 169 | 170 | # Select the next generation individuals 171 | log('[GA] Selecting the next generation') 172 | offspring = toolbox.select(pop, len(pop)) 173 | # Clone the selected individuals 174 | offspring = list(map(toolbox.clone, offspring)) 175 | 176 | # Apply crossover and mutation on the offspring 177 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 178 | # cross two individuals with probability CXPB 179 | if random.random() < CXPB: 180 | toolbox.mate(child1, child2) 181 | 182 | # fitness values of the children 183 | # must be recalculated later 184 | del child1.fitness.values 185 | del child2.fitness.values 186 | 187 | for mutant in offspring: 188 | # mutate an individual with probability MUTPB 189 | if random.random() < MUTPB: 190 | toolbox.mutate(mutant) 191 | del mutant.fitness.values 192 | 193 | # Since the content of some of our offspring changed during the last step, we now need to 194 | # re-evaluate their fitnesses. To save time and resources, we just map those offspring which 195 | # fitnesses were marked invalid. 196 | # Evaluate the individuals with an invalid fitness 197 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 198 | fitnesses = map(toolbox.evaluate, invalid_ind) 199 | for ind, fit in zip(invalid_ind, fitnesses): 200 | ind.fitness.values = (fit,) 201 | 202 | log('[GA] Evaluated {0} individuals (invalid fitness)'.format(len(invalid_ind))) 203 | 204 | # The population is entirely replaced by the offspring 205 | pop[:] = offspring 206 | 207 | # Gather all the fitnesses in one list and print the stats 208 | fits = [ind.fitness.values[0] for ind in pop] 209 | 210 | length = len(pop) 211 | mean = sum(fits) / length 212 | sum2 = sum(x * x for x in fits) 213 | std = abs(sum2 / length - mean ** 2) ** 0.5 214 | 215 | log('[GA] Results for generation {0}'.format(g)) 216 | log('[GA] Min %s' % min(fits)) 217 | log('[GA] Max %s' % max(fits)) 218 | log('[GA] Avg %s' % mean) 219 | log('[GA] Std %s' % std) 220 | 221 | best_ind_g = tools.selBest(pop, 1)[0] 222 | 223 | # Store the best individual over all generations 224 | if best_ind_g.fitness.values[0] > best_fitness_ever: 225 | best_fitness_ever = best_ind_g.fitness.values[0] 226 | best_ind_ever = best_ind_g 227 | 228 | log('[GA] Best individual for generation {0}: {1}, {2}'.format(g, best_ind_g, best_ind_g.fitness.values[0])) 229 | log('[GA] Best individual ever till now: %s, %s' % (best_ind_ever, best_fitness_ever)) 230 | 231 | log('[GA] ############################# End of generation {0} #############################'.format(g)) 232 | 233 | log('[GA] ===================== End of evolution =====================') 234 | 235 | best_ind = tools.selBest(pop, 1)[0] 236 | log('[GA] Best individual in the population: %s, %s' % (best_ind, best_ind.fitness.values[0])) 237 | log('[GA] Best individual ever: %s, %s' % (best_ind_ever, best_fitness_ever)) 238 | 239 | if __name__ == "__main__": 240 | main() 241 | -------------------------------------------------------------------------------- /matsuoka_walk/ga_5.py: -------------------------------------------------------------------------------- 1 | # Script with low level feedback for phase resetting of pacemaker oscillator 2 | 3 | import os 4 | import random 5 | 6 | from deap import base 7 | from deap import creator 8 | from deap import tools 9 | 10 | from matsuoka_walk import Logger, log 11 | from matsuoka_walk.oscillator_5 import oscillator_nw 12 | 13 | # Set the home directory 14 | home_dir = os.path.expanduser('~') 15 | 16 | # Set the logging variables 17 | # This also creates a new log file 18 | Logger(log_dir=os.path.join(home_dir, '.bio_walk/logs/'), log_flag=True) 19 | 20 | log('[GA] Running ga_5') 21 | 22 | # Create the position bounds of the individual 23 | log('[GA] Creating position bounds') 24 | FLT_MIN_KF, FLT_MAX_KF = 0.2, 1.0 25 | FLT_MIN_GAIN1, FLT_MAX_GAIN1 = 0.01, 1.0 26 | FLT_MIN_GAIN2, FLT_MAX_GAIN2 = 0.01, 1.0 27 | FLT_MIN_GAIN3, FLT_MAX_GAIN3 = 0.01, 1.0 28 | FLT_MIN_GAIN4, FLT_MAX_GAIN4 = 0.01, 1.0 29 | FLT_MIN_GAIN5, FLT_MAX_GAIN5 = 0.01, 1.0 30 | FLT_MIN_GAIN6, FLT_MAX_GAIN6 = 0.01, 1.0 31 | FLT_MIN_BIAS1, FLT_MAX_BIAS1 = -0.6, 0.0 32 | FLT_MIN_BIAS2, FLT_MAX_BIAS2 = 0.0, 0.5 33 | FLT_MIN_BIAS3, FLT_MAX_BIAS3 = -0.5, 0.0 34 | FLT_MIN_BIAS4, FLT_MAX_BIAS4 = 0.0, 1.0 35 | 36 | log('[GA] Logging position bounds') 37 | log('[GA] FLT_MIN_KF={0}, FLT_MAX_KF={1}'.format(FLT_MIN_KF, FLT_MAX_KF)) 38 | log('[GA] FLT_MIN_GAIN1={0}, FLT_MAX_GAIN1={1}'.format(FLT_MIN_GAIN1, FLT_MAX_GAIN1)) 39 | log('[GA] FLT_MIN_GAIN2={0}, FLT_MAX_GAIN2={1}'.format(FLT_MIN_GAIN2, FLT_MAX_GAIN2)) 40 | log('[GA] FLT_MIN_GAIN3={0}, FLT_MAX_GAIN3={1}'.format(FLT_MIN_GAIN3, FLT_MAX_GAIN3)) 41 | log('[GA] FLT_MIN_GAIN4={0}, FLT_MAX_GAIN4={1}'.format(FLT_MIN_GAIN4, FLT_MAX_GAIN4)) 42 | log('[GA] FLT_MIN_GAIN5={0}, FLT_MAX_GAIN5={1}'.format(FLT_MIN_GAIN5, FLT_MAX_GAIN5)) 43 | log('[GA] FLT_MIN_GAIN6={0}, FLT_MAX_GAIN6={1}'.format(FLT_MIN_GAIN6, FLT_MAX_GAIN6)) 44 | log('[GA] FLT_MIN_BIAS1={0}, FLT_MAX_BIAS1={1}'.format(FLT_MIN_BIAS1, FLT_MAX_BIAS1)) 45 | log('[GA] FLT_MIN_BIAS2={0}, FLT_MAX_BIAS2={1}'.format(FLT_MIN_BIAS2, FLT_MAX_BIAS2)) 46 | log('[GA] FLT_MIN_BIAS3={0}, FLT_MAX_BIAS3={1}'.format(FLT_MIN_BIAS3, FLT_MAX_BIAS3)) 47 | log('[GA] FLT_MIN_BIAS4={0}, FLT_MAX_BIAS4={1}'.format(FLT_MIN_BIAS4, FLT_MAX_BIAS4)) 48 | 49 | # Define a custom class named `FitnessMax` 50 | # Single objective function is specified by the tuple `weights=(1.0,)` 51 | creator.create("FitnessMax", base.Fitness, weights=(1.0,)) 52 | 53 | # Create a class named `Individual` which inherits from the class `list` and has `FitnessMax` as an attribute 54 | creator.create("Individual", list, fitness=creator.FitnessMax) 55 | 56 | # Now we will use our custom classes to create types representing our individuals as well as our whole population. 57 | # All the objects we will use on our way, an individual, the population, as well as all functions, operators, and 58 | # arguments will be stored in a DEAP container called `Toolbox`. It contains two methods for adding and removing 59 | # content, register() and unregister(). 60 | 61 | toolbox = base.Toolbox() 62 | 63 | # Attribute generator - specify how each single gene is to be created 64 | toolbox.register("kf_flt", random.uniform, FLT_MIN_KF, FLT_MAX_KF) 65 | toolbox.register("gain1_flt", random.uniform, FLT_MIN_GAIN1, FLT_MAX_GAIN1) 66 | toolbox.register("gain2_flt", random.uniform, FLT_MIN_GAIN2, FLT_MAX_GAIN2) 67 | toolbox.register("gain3_flt", random.uniform, FLT_MIN_GAIN3, FLT_MAX_GAIN3) 68 | toolbox.register("gain4_flt", random.uniform, FLT_MIN_GAIN4, FLT_MAX_GAIN4) 69 | toolbox.register("gain5_flt", random.uniform, FLT_MIN_GAIN5, FLT_MAX_GAIN5) 70 | toolbox.register("gain6_flt", random.uniform, FLT_MIN_GAIN6, FLT_MAX_GAIN6) 71 | toolbox.register("bias1_flt", random.uniform, FLT_MIN_BIAS1, FLT_MAX_BIAS1) 72 | toolbox.register("bias2_flt", random.uniform, FLT_MIN_BIAS2, FLT_MAX_BIAS2) 73 | toolbox.register("bias3_flt", random.uniform, FLT_MIN_BIAS3, FLT_MAX_BIAS3) 74 | toolbox.register("bias4_flt", random.uniform, FLT_MIN_BIAS4, FLT_MAX_BIAS4) 75 | 76 | # Specify the structure of an individual chromosome 77 | N_CYCLES=1 # Number of times to repeat this pattern 78 | 79 | # Specify the sequence of genes in an individual chromosome 80 | toolbox.register("individual", tools.initCycle, creator.Individual, 81 | (toolbox.kf_flt, 82 | toolbox.gain1_flt, toolbox.gain2_flt, toolbox.gain3_flt, 83 | toolbox.gain4_flt, toolbox.gain5_flt, toolbox.gain6_flt, 84 | toolbox.bias1_flt ,toolbox.bias2_flt, toolbox.bias3_flt, toolbox.bias4_flt), 85 | n=N_CYCLES) 86 | 87 | # Define the population to be a list of individuals 88 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 89 | 90 | # Register the goal / fitness function 91 | toolbox.register("evaluate", oscillator_nw) 92 | 93 | # Register the crossover operator - 2 point crossover is used here 94 | toolbox.register("mate", tools.cxTwoPoint) 95 | 96 | # Register a mutation operator 97 | # Mutation is done by adding a float to each gene. This float to be added is randomly selected from a Gaussian 98 | # distribution with mu=0.0 and sigma=0.01 99 | # Probability of mutation is 0.05 100 | toolbox.register("mutate", tools.mutGaussian, mu=0.0, sigma=0.01, indpb=0.05) 101 | 102 | # Operator for selecting individuals for breeding the next 103 | # generation: each individual of the current generation 104 | # is replaced by the 'fittest' (best) of 3 individuals 105 | # drawn randomly from the current generation. 106 | toolbox.register("select", tools.selTournament, tournsize=3) 107 | 108 | # Size of the population 109 | POP_SIZE = 200 110 | 111 | # Maximum generations 112 | MAX_GEN = 32 113 | 114 | def main(): 115 | #random.seed(64) 116 | 117 | # Create an initial population of `POP_SIZE` individuals (where each individual is a list of floats) 118 | pop = toolbox.population(n=POP_SIZE) 119 | 120 | # CXPB is the probability with which two individuals are crossed 121 | # MUTPB is the probability for mutating an individual 122 | CXPB, MUTPB = 0.8, 0.1 123 | 124 | log('[GA] Starting genetic algorithm') 125 | 126 | # Evaluate the entire population and store the fitness of each individual 127 | log('[GA] Finding the fitness of individuals in the initial generation') 128 | fitnesses = list(map(toolbox.evaluate, pop)) 129 | for ind, fit in zip(pop, fitnesses): 130 | print ind, fit 131 | ind.fitness.values = (fit,) 132 | 133 | # Extracting all the fitnesses 134 | fits = [ind.fitness.values[0] for ind in pop] 135 | 136 | # Variable keeping track of the number of generations 137 | g = 0 138 | 139 | best_ind_ever = None 140 | best_fitness_ever = 0.0 141 | 142 | # Begin the evolution 143 | while max(fits) < 100 and g < MAX_GEN: 144 | 145 | # A new generation 146 | g = g + 1 147 | log('[GA] Running generation {0}'.format(g)) 148 | 149 | # Select the next generation individuals 150 | log('[GA] Selecting the next generation') 151 | offspring = toolbox.select(pop, len(pop)) 152 | # Clone the selected individuals 153 | offspring = list(map(toolbox.clone, offspring)) 154 | 155 | # Apply crossover and mutation on the offspring 156 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 157 | # cross two individuals with probability CXPB 158 | if random.random() < CXPB: 159 | toolbox.mate(child1, child2) 160 | 161 | # fitness values of the children 162 | # must be recalculated later 163 | del child1.fitness.values 164 | del child2.fitness.values 165 | 166 | for mutant in offspring: 167 | # mutate an individual with probability MUTPB 168 | if random.random() < MUTPB: 169 | toolbox.mutate(mutant) 170 | del mutant.fitness.values 171 | 172 | # Since the content of some of our offspring changed during the last step, we now need to 173 | # re-evaluate their fitnesses. To save time and resources, we just map those offspring which 174 | # fitnesses were marked invalid. 175 | # Evaluate the individuals with an invalid fitness 176 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 177 | fitnesses = map(toolbox.evaluate, invalid_ind) 178 | for ind, fit in zip(invalid_ind, fitnesses): 179 | ind.fitness.values = (fit,) 180 | 181 | log('[GA] Evaluated {0} individuals (invalid fitness)'.format(len(invalid_ind))) 182 | 183 | # The population is entirely replaced by the offspring 184 | pop[:] = offspring 185 | 186 | # Gather all the fitnesses in one list and print the stats 187 | fits = [ind.fitness.values[0] for ind in pop] 188 | 189 | length = len(pop) 190 | mean = sum(fits) / length 191 | sum2 = sum(x * x for x in fits) 192 | std = abs(sum2 / length - mean ** 2) ** 0.5 193 | 194 | log('[GA] Results for generation {0}'.format(g)) 195 | log('[GA] Min %s' % min(fits)) 196 | log('[GA] Max %s' % max(fits)) 197 | log('[GA] Avg %s' % mean) 198 | log('[GA] Std %s' % std) 199 | 200 | best_ind_g = tools.selBest(pop, 1)[0] 201 | 202 | # Store the best individual over all generations 203 | if best_ind_g.fitness.values[0] > best_fitness_ever: 204 | best_fitness_ever = best_ind_g.fitness.values[0] 205 | best_ind_ever = best_ind_g 206 | 207 | log('[GA] Best individual for generation {0}: {1}, {2}'.format(g, best_ind_g, best_ind_g.fitness.values[0])) 208 | log('[GA] Best individual ever till now: %s, %s' % (best_ind_ever, best_fitness_ever)) 209 | 210 | log('[GA] ############################# End of generation {0} #############################'.format(g)) 211 | 212 | log('[GA] ===================== End of evolution =====================') 213 | 214 | best_ind = tools.selBest(pop, 1)[0] 215 | log('[GA] Best individual in the population: %s, %s' % (best_ind, best_ind.fitness.values[0])) 216 | log('[GA] Best individual ever: %s, %s' % (best_ind_ever, best_fitness_ever)) 217 | 218 | if __name__ == "__main__": 219 | main() 220 | -------------------------------------------------------------------------------- /matsuoka_walk/ga_5_1.py: -------------------------------------------------------------------------------- 1 | # Script with low level feedback for phase resetting of pacemaker oscillator without ROS 2 | 3 | import os 4 | import random 5 | 6 | from deap import base 7 | from deap import creator 8 | from deap import tools 9 | 10 | from matsuoka_walk import Logger, log 11 | from matsuoka_walk.oscillator_5_1 import oscillator_nw 12 | 13 | # Set the home directory 14 | home_dir = os.path.expanduser('~') 15 | 16 | # Set the logging variables 17 | # This also creates a new log file 18 | Logger(log_dir=os.path.join(home_dir, '.bio_walk/logs/'), log_flag=True) 19 | 20 | log('[GA] Running ga_5') 21 | 22 | # Create the position bounds of the individual 23 | log('[GA] Creating position bounds') 24 | FLT_MIN_KF, FLT_MAX_KF = 0.2, 1.0 25 | FLT_MIN_GAIN1, FLT_MAX_GAIN1 = 0.01, 1.0 26 | FLT_MIN_GAIN2, FLT_MAX_GAIN2 = 0.01, 1.0 27 | FLT_MIN_GAIN3, FLT_MAX_GAIN3 = 0.01, 1.0 28 | FLT_MIN_GAIN4, FLT_MAX_GAIN4 = 0.01, 1.0 29 | FLT_MIN_GAIN5, FLT_MAX_GAIN5 = 0.01, 1.0 30 | FLT_MIN_GAIN6, FLT_MAX_GAIN6 = 0.01, 1.0 31 | FLT_MIN_BIAS1, FLT_MAX_BIAS1 = -0.6, 0.0 32 | FLT_MIN_BIAS2, FLT_MAX_BIAS2 = 0.0, 0.5 33 | FLT_MIN_BIAS3, FLT_MAX_BIAS3 = -0.5, 0.0 34 | FLT_MIN_BIAS4, FLT_MAX_BIAS4 = 0.0, 1.0 35 | 36 | log('[GA] Logging position bounds') 37 | log('[GA] FLT_MIN_KF={0}, FLT_MAX_KF={1}'.format(FLT_MIN_KF, FLT_MAX_KF)) 38 | log('[GA] FLT_MIN_GAIN1={0}, FLT_MAX_GAIN1={1}'.format(FLT_MIN_GAIN1, FLT_MAX_GAIN1)) 39 | log('[GA] FLT_MIN_GAIN2={0}, FLT_MAX_GAIN2={1}'.format(FLT_MIN_GAIN2, FLT_MAX_GAIN2)) 40 | log('[GA] FLT_MIN_GAIN3={0}, FLT_MAX_GAIN3={1}'.format(FLT_MIN_GAIN3, FLT_MAX_GAIN3)) 41 | log('[GA] FLT_MIN_GAIN4={0}, FLT_MAX_GAIN4={1}'.format(FLT_MIN_GAIN4, FLT_MAX_GAIN4)) 42 | log('[GA] FLT_MIN_GAIN5={0}, FLT_MAX_GAIN5={1}'.format(FLT_MIN_GAIN5, FLT_MAX_GAIN5)) 43 | log('[GA] FLT_MIN_GAIN6={0}, FLT_MAX_GAIN6={1}'.format(FLT_MIN_GAIN6, FLT_MAX_GAIN6)) 44 | log('[GA] FLT_MIN_BIAS1={0}, FLT_MAX_BIAS1={1}'.format(FLT_MIN_BIAS1, FLT_MAX_BIAS1)) 45 | log('[GA] FLT_MIN_BIAS2={0}, FLT_MAX_BIAS2={1}'.format(FLT_MIN_BIAS2, FLT_MAX_BIAS2)) 46 | log('[GA] FLT_MIN_BIAS3={0}, FLT_MAX_BIAS3={1}'.format(FLT_MIN_BIAS3, FLT_MAX_BIAS3)) 47 | log('[GA] FLT_MIN_BIAS4={0}, FLT_MAX_BIAS4={1}'.format(FLT_MIN_BIAS4, FLT_MAX_BIAS4)) 48 | 49 | # Define a custom class named `FitnessMax` 50 | # Single objective function is specified by the tuple `weights=(1.0,)` 51 | creator.create("FitnessMax", base.Fitness, weights=(1.0,)) 52 | 53 | # Create a class named `Individual` which inherits from the class `list` and has `FitnessMax` as an attribute 54 | creator.create("Individual", list, fitness=creator.FitnessMax) 55 | 56 | # Now we will use our custom classes to create types representing our individuals as well as our whole population. 57 | # All the objects we will use on our way, an individual, the population, as well as all functions, operators, and 58 | # arguments will be stored in a DEAP container called `Toolbox`. It contains two methods for adding and removing 59 | # content, register() and unregister(). 60 | 61 | toolbox = base.Toolbox() 62 | 63 | # Attribute generator - specify how each single gene is to be created 64 | toolbox.register("kf_flt", random.uniform, FLT_MIN_KF, FLT_MAX_KF) 65 | toolbox.register("gain1_flt", random.uniform, FLT_MIN_GAIN1, FLT_MAX_GAIN1) 66 | toolbox.register("gain2_flt", random.uniform, FLT_MIN_GAIN2, FLT_MAX_GAIN2) 67 | toolbox.register("gain3_flt", random.uniform, FLT_MIN_GAIN3, FLT_MAX_GAIN3) 68 | toolbox.register("gain4_flt", random.uniform, FLT_MIN_GAIN4, FLT_MAX_GAIN4) 69 | toolbox.register("gain5_flt", random.uniform, FLT_MIN_GAIN5, FLT_MAX_GAIN5) 70 | toolbox.register("gain6_flt", random.uniform, FLT_MIN_GAIN6, FLT_MAX_GAIN6) 71 | toolbox.register("bias1_flt", random.uniform, FLT_MIN_BIAS1, FLT_MAX_BIAS1) 72 | toolbox.register("bias2_flt", random.uniform, FLT_MIN_BIAS2, FLT_MAX_BIAS2) 73 | toolbox.register("bias3_flt", random.uniform, FLT_MIN_BIAS3, FLT_MAX_BIAS3) 74 | toolbox.register("bias4_flt", random.uniform, FLT_MIN_BIAS4, FLT_MAX_BIAS4) 75 | 76 | # Specify the structure of an individual chromosome 77 | N_CYCLES=1 # Number of times to repeat this pattern 78 | 79 | # Specify the sequence of genes in an individual chromosome 80 | toolbox.register("individual", tools.initCycle, creator.Individual, 81 | (toolbox.kf_flt, 82 | toolbox.gain1_flt, toolbox.gain2_flt, toolbox.gain3_flt, 83 | toolbox.gain4_flt, toolbox.gain5_flt, toolbox.gain6_flt, 84 | toolbox.bias1_flt ,toolbox.bias2_flt, toolbox.bias3_flt, toolbox.bias4_flt), 85 | n=N_CYCLES) 86 | 87 | # Define the population to be a list of individuals 88 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 89 | 90 | # Register the goal / fitness function 91 | toolbox.register("evaluate", oscillator_nw) 92 | 93 | # Register the crossover operator - 2 point crossover is used here 94 | toolbox.register("mate", tools.cxTwoPoint) 95 | 96 | # Register a mutation operator 97 | # Mutation is done by adding a float to each gene. This float to be added is randomly selected from a Gaussian 98 | # distribution with mu=0.0 and sigma=0.01 99 | # Probability of mutation is 0.05 100 | toolbox.register("mutate", tools.mutGaussian, mu=0.0, sigma=0.01, indpb=0.05) 101 | 102 | # Operator for selecting individuals for breeding the next 103 | # generation: each individual of the current generation 104 | # is replaced by the 'fittest' (best) of 3 individuals 105 | # drawn randomly from the current generation. 106 | toolbox.register("select", tools.selTournament, tournsize=3) 107 | 108 | # Size of the population 109 | POP_SIZE = 200 110 | 111 | # Maximum generations 112 | MAX_GEN = 32 113 | 114 | def main(): 115 | #random.seed(64) 116 | 117 | # Create an initial population of `POP_SIZE` individuals (where each individual is a list of floats) 118 | pop = toolbox.population(n=POP_SIZE) 119 | 120 | # CXPB is the probability with which two individuals are crossed 121 | # MUTPB is the probability for mutating an individual 122 | CXPB, MUTPB = 0.8, 0.1 123 | 124 | log('[GA] Starting genetic algorithm') 125 | 126 | # Evaluate the entire population and store the fitness of each individual 127 | log('[GA] Finding the fitness of individuals in the initial generation') 128 | fitnesses = list(map(toolbox.evaluate, pop)) 129 | for ind, fit in zip(pop, fitnesses): 130 | print ind, fit 131 | ind.fitness.values = (fit,) 132 | 133 | # Extracting all the fitnesses 134 | fits = [ind.fitness.values[0] for ind in pop] 135 | 136 | # Variable keeping track of the number of generations 137 | g = 0 138 | 139 | best_ind_ever = None 140 | best_fitness_ever = 0.0 141 | 142 | # Begin the evolution 143 | while max(fits) < 100 and g < MAX_GEN: 144 | 145 | # A new generation 146 | g = g + 1 147 | log('[GA] Running generation {0}'.format(g)) 148 | 149 | # Select the next generation individuals 150 | log('[GA] Selecting the next generation') 151 | offspring = toolbox.select(pop, len(pop)) 152 | # Clone the selected individuals 153 | offspring = list(map(toolbox.clone, offspring)) 154 | 155 | # Apply crossover and mutation on the offspring 156 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 157 | # cross two individuals with probability CXPB 158 | if random.random() < CXPB: 159 | toolbox.mate(child1, child2) 160 | 161 | # fitness values of the children 162 | # must be recalculated later 163 | del child1.fitness.values 164 | del child2.fitness.values 165 | 166 | for mutant in offspring: 167 | # mutate an individual with probability MUTPB 168 | if random.random() < MUTPB: 169 | toolbox.mutate(mutant) 170 | del mutant.fitness.values 171 | 172 | # Since the content of some of our offspring changed during the last step, we now need to 173 | # re-evaluate their fitnesses. To save time and resources, we just map those offspring which 174 | # fitnesses were marked invalid. 175 | # Evaluate the individuals with an invalid fitness 176 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 177 | fitnesses = map(toolbox.evaluate, invalid_ind) 178 | for ind, fit in zip(invalid_ind, fitnesses): 179 | ind.fitness.values = (fit,) 180 | 181 | log('[GA] Evaluated {0} individuals (invalid fitness)'.format(len(invalid_ind))) 182 | 183 | # The population is entirely replaced by the offspring 184 | pop[:] = offspring 185 | 186 | # Gather all the fitnesses in one list and print the stats 187 | fits = [ind.fitness.values[0] for ind in pop] 188 | 189 | length = len(pop) 190 | mean = sum(fits) / length 191 | sum2 = sum(x * x for x in fits) 192 | std = abs(sum2 / length - mean ** 2) ** 0.5 193 | 194 | log('[GA] Results for generation {0}'.format(g)) 195 | log('[GA] Min %s' % min(fits)) 196 | log('[GA] Max %s' % max(fits)) 197 | log('[GA] Avg %s' % mean) 198 | log('[GA] Std %s' % std) 199 | 200 | best_ind_g = tools.selBest(pop, 1)[0] 201 | 202 | # Store the best individual over all generations 203 | if best_ind_g.fitness.values[0] > best_fitness_ever: 204 | best_fitness_ever = best_ind_g.fitness.values[0] 205 | best_ind_ever = best_ind_g 206 | 207 | log('[GA] Best individual for generation {0}: {1}, {2}'.format(g, best_ind_g, best_ind_g.fitness.values[0])) 208 | log('[GA] Best individual ever till now: %s, %s' % (best_ind_ever, best_fitness_ever)) 209 | 210 | log('[GA] ############################# End of generation {0} #############################'.format(g)) 211 | 212 | log('[GA] ===================== End of evolution =====================') 213 | 214 | best_ind = tools.selBest(pop, 1)[0] 215 | log('[GA] Best individual in the population: %s, %s' % (best_ind, best_ind.fitness.values[0])) 216 | log('[GA] Best individual ever: %s, %s' % (best_ind_ever, best_fitness_ever)) 217 | 218 | if __name__ == "__main__": 219 | main() 220 | -------------------------------------------------------------------------------- /matsuoka_walk/ga_test.py: -------------------------------------------------------------------------------- 1 | import random 2 | 3 | from deap import base 4 | from deap import creator 5 | from deap import tools 6 | 7 | from matsuoka_walk.fitness import hart6sc 8 | 9 | creator.create("FitnessMin", base.Fitness, weights=(-1.0,)) 10 | creator.create("Individual", list, fitness=creator.FitnessMin) 11 | 12 | toolbox = base.Toolbox() 13 | 14 | FLT_MIN_1, FLT_MAX_1 = 0.0, 1.0 15 | FLT_MIN_2, FLT_MAX_2 = 0.0, 1.0 16 | FLT_MIN_3, FLT_MAX_3 = 0.0, 1.0 17 | FLT_MIN_4, FLT_MAX_4 = 0.0, 1.0 18 | FLT_MIN_5, FLT_MAX_5 = 0.0, 1.0 19 | FLT_MIN_6, FLT_MAX_6 = 0.0, 1.0 20 | 21 | # Attribute generator 22 | # define 'attr_bool' to be an attribute ('gene') 23 | # which corresponds to integers sampled uniformly 24 | # from the range [0,1] (i.e. 0 or 1 with equal 25 | # probability) 26 | #toolbox.register("attr_bool", random.randint, 0, 1) 27 | 28 | toolbox.register("attr_flt_1", random.uniform, FLT_MIN_1, FLT_MAX_1) 29 | toolbox.register("attr_flt_2", random.uniform, FLT_MIN_2, FLT_MAX_2) 30 | toolbox.register("attr_flt_3", random.uniform, FLT_MIN_3, FLT_MAX_3) 31 | toolbox.register("attr_flt_4", random.uniform, FLT_MIN_4, FLT_MAX_4) 32 | toolbox.register("attr_flt_5", random.uniform, FLT_MIN_5, FLT_MAX_5) 33 | toolbox.register("attr_flt_6", random.uniform, FLT_MIN_6, FLT_MAX_6) 34 | 35 | N_CYCLES=1 36 | 37 | toolbox.register("individual", tools.initCycle, creator.Individual, 38 | (toolbox.attr_flt_1, toolbox.attr_flt_2, toolbox.attr_flt_3, 39 | toolbox.attr_flt_4, toolbox.attr_flt_5, toolbox.attr_flt_6), 40 | n=N_CYCLES) 41 | 42 | # define the population to be a list of individuals 43 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 44 | 45 | # Structure initializers 46 | # define 'individual' to be an individual 47 | # consisting of 100 'attr_bool' elements ('genes') 48 | #toolbox.register("individual", tools.initRepeat, creator.Individual, toolbox.attr_bool, 100) 49 | 50 | # define the population to be a list of individuals 51 | #toolbox.register("population", tools.initRepeat, list, toolbox.individual) 52 | 53 | 54 | # the goal ('fitness') function to be maximized 55 | def evalOneMax(individual): 56 | return sum(individual), 57 | 58 | 59 | # ---------- 60 | # Operator registration 61 | # ---------- 62 | # register the goal / fitness function 63 | toolbox.register("evaluate", hart6sc) 64 | 65 | # register the crossover operator 66 | toolbox.register("mate", tools.cxTwoPoint) 67 | 68 | # register a mutation operator with a probability to 69 | # flip each attribute/gene of 0.05 70 | toolbox.register("mutate", tools.mutGaussian, mu=0.0, sigma=0.01, indpb=0.05) 71 | 72 | # operator for selecting individuals for breeding the next 73 | # generation: each individual of the current generation 74 | # is replaced by the 'fittest' (best) of three individuals 75 | # drawn randomly from the current generation. 76 | toolbox.register("select", tools.selTournament, tournsize=3) 77 | 78 | 79 | # ---------- 80 | 81 | def main(): 82 | #random.seed(64) 83 | 84 | # create an initial population of 300 individuals (where 85 | # each individual is a list of integers) 86 | pop = toolbox.population(n=300) 87 | 88 | # CXPB is the probability with which two individuals 89 | # are crossed 90 | # 91 | # MUTPB is the probability for mutating an individual 92 | CXPB, MUTPB = 0.8, 0.1 93 | 94 | print("Start of evolution") 95 | 96 | # Evaluate the entire population 97 | fitnesses = list(map(toolbox.evaluate, pop)) 98 | for ind, fit in zip(pop, fitnesses): 99 | ind.fitness.values = (fit,) 100 | 101 | print(" Evaluated %i individuals" % len(pop)) 102 | 103 | # Extracting all the fitnesses of 104 | fits = [ind.fitness.values[0] for ind in pop] 105 | 106 | # Variable keeping track of the number of generations 107 | g = 0 108 | 109 | # Begin the evolution 110 | while max(fits) < 100 and g < 200: 111 | # A new generation 112 | g = g + 1 113 | print("-- Generation %i --" % g) 114 | 115 | # Select the next generation individuals 116 | offspring = toolbox.select(pop, len(pop)) 117 | # Clone the selected individuals 118 | offspring = list(map(toolbox.clone, offspring)) 119 | 120 | # Apply crossover and mutation on the offspring 121 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 122 | 123 | # cross two individuals with probability CXPB 124 | if random.random() < CXPB: 125 | toolbox.mate(child1, child2) 126 | 127 | # fitness values of the children 128 | # must be recalculated later 129 | del child1.fitness.values 130 | del child2.fitness.values 131 | 132 | for mutant in offspring: 133 | 134 | # mutate an individual with probability MUTPB 135 | if random.random() < MUTPB: 136 | toolbox.mutate(mutant) 137 | del mutant.fitness.values 138 | 139 | # Evaluate the individuals with an invalid fitness 140 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 141 | fitnesses = map(toolbox.evaluate, invalid_ind) 142 | for ind, fit in zip(invalid_ind, fitnesses): 143 | ind.fitness.values = (fit,) 144 | 145 | print(" Evaluated %i individuals" % len(invalid_ind)) 146 | 147 | # The population is entirely replaced by the offspring 148 | pop[:] = offspring 149 | 150 | # Gather all the fitnesses in one list and print the stats 151 | fits = [ind.fitness.values[0] for ind in pop] 152 | 153 | length = len(pop) 154 | mean = sum(fits) / length 155 | sum2 = sum(x * x for x in fits) 156 | std = abs(sum2 / length - mean ** 2) ** 0.5 157 | 158 | print(" Min %s" % min(fits)) 159 | print(" Max %s" % max(fits)) 160 | print(" Avg %s" % mean) 161 | print(" Std %s" % std) 162 | 163 | print("-- End of (successful) evolution --") 164 | 165 | best_ind = tools.selBest(pop, 1)[0] 166 | print("Best individual is %s, %s" % (best_ind, best_ind.fitness.values)) 167 | 168 | 169 | if __name__ == "__main__": 170 | main() 171 | -------------------------------------------------------------------------------- /matsuoka_walk/gait_eval_result.py: -------------------------------------------------------------------------------- 1 | """ 2 | Stores temporary results from the monitor 3 | """ 4 | 5 | 6 | class GaitEvalResult: 7 | """ 8 | Class for storing global gait evaluation related variables 9 | """ 10 | fallen = True 11 | up_time = 0.0 12 | x = 0.0 13 | y = 0.0 14 | avg_footstep = 0.0 15 | var_torso_orientation_alpha = 0.0 16 | var_torso_orientation_beta = 0.0 17 | var_torso_orientation_gamma = 0.0 18 | 19 | def __init__(self, log_dir, log_flag=True): 20 | GaitEvalResult.fallen = True 21 | GaitEvalResult.up_time = 0.0 22 | GaitEvalResult.x = 0.0 23 | GaitEvalResult.y = 0.0 24 | GaitEvalResult.avg_footstep = 0.0 25 | GaitEvalResult.var_torso_orientation_alpha = 0.0 26 | GaitEvalResult.var_torso_orientation_beta = 0.0 27 | GaitEvalResult.var_torso_orientation_gamma = 0.0 28 | 29 | -------------------------------------------------------------------------------- /matsuoka_walk/log.py: -------------------------------------------------------------------------------- 1 | """ 2 | Module for logging related functions 3 | """ 4 | 5 | import datetime 6 | import os 7 | 8 | 9 | class Logger: 10 | """ 11 | Class for storing global logging related variables 12 | """ 13 | log_file = None 14 | log_dir = None 15 | log_flag = True 16 | datetime_str = None 17 | 18 | def __init__(self, log_dir, log_flag=True): 19 | """ 20 | Function to initialize the logging variables and to create a log file 21 | 22 | :param log_dir: The directory to save log files 23 | :param log_flag: If True, print on console and also save to file, else only save to file 24 | """ 25 | 26 | if not os.path.exists(log_dir): 27 | raise ValueError('Log directory ' + log_dir + ' does not exist') 28 | 29 | # Find the datetime string 30 | datetime_str = datetime.datetime.now().strftime('%Y%m%d_%H%M%S') 31 | Logger.datetime_str = datetime_str 32 | 33 | # Set the static global variables 34 | Logger.log_dir = os.path.join(log_dir, datetime_str) 35 | Logger.log_flag = log_flag 36 | 37 | # Create directory 38 | os.makedirs(Logger.log_dir) 39 | 40 | # Create log file name 41 | filename = 'log_' + datetime_str + '.txt' 42 | 43 | # Set in Globals 44 | Logger.log_file = os.path.join(Logger.log_dir, filename) 45 | 46 | # Create the log file 47 | open(Logger.log_file, 'a').close() 48 | 49 | 50 | def log(logstr): 51 | """ 52 | Print logstr with a timestamp to the console and store in the log file 53 | 54 | :param logstr: String to print 55 | :return: 56 | """ 57 | 58 | logstr_fmt = ("[%s] %s \n" % (datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3], logstr)) 59 | 60 | if Logger.log_flag: 61 | print logstr_fmt 62 | 63 | fh = open(Logger.log_file, 'a') 64 | fh.write(logstr_fmt) 65 | fh.close() 66 | 67 | -------------------------------------------------------------------------------- /matsuoka_walk/matsuoka2011.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | matplotlib.use('TkAgg') 4 | 5 | import matplotlib.pyplot as plt 6 | 7 | # Constants 8 | a = 2.5 9 | b = 2.5 10 | c = 1.5 11 | tau = 0.25 12 | T = 0.5 13 | 14 | dt = 0.01 15 | 16 | # Variables 17 | x1 = 0.5 18 | x2 = 0.0 19 | v1 = 0.0 20 | v2 = 0.0 21 | y1 = 0.0 22 | y2 = 0.0 23 | 24 | x1_list = list() 25 | x2_list = list() 26 | y1_list = list() 27 | y2_list = list() 28 | t_list = list() 29 | 30 | for t in np.arange(0.0, 10.0, dt): 31 | 32 | d_x1_dt = (-x1 + c - a*y2 -b*v1)/tau 33 | d_v1_dt = (-v1 + y1)/T 34 | y1 = max([0.0, x1]) 35 | 36 | d_x2_dt = (-x2 + c - a*y1 -b*v2)/tau 37 | d_v2_dt = (-v2 + y2)/T 38 | y2 = max([0.0, x2]) 39 | 40 | x1 += d_x1_dt * dt 41 | x2 += d_x2_dt * dt 42 | v1 += d_v1_dt * dt 43 | v2 += d_v2_dt * dt 44 | 45 | x1_list.append(x1) 46 | y1_list.append(y1) 47 | t_list.append(t) 48 | 49 | plt.figure(1) 50 | plt.plot(t_list, x1_list, color='red') 51 | plt.plot(t_list, y1_list, color='blue') 52 | plt.xticks(np.arange(0.0, 10.0, 1.0)) 53 | plt.yticks(np.arange(-1.0, 1.0, 0.1)) 54 | plt.show() 55 | 56 | -------------------------------------------------------------------------------- /matsuoka_walk/matsuoka_env.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import gym 3 | from gym import spaces 4 | import time 5 | 6 | from matsuoka_walk.log import log 7 | from matsuoka_walk.oscillator_3_thread import Oscillator3Thread 8 | 9 | # Factors which decide the weightage of deviation and forward distance on the reward calculation 10 | # The reward is calculated as: reward = ALPHA*(-abs(deviation)) + BETA*forward_distance + THETA*orientation 11 | 12 | ALPHA = 1.0 13 | BETA = 0.3 14 | THETA = 1.0 15 | 16 | class MatsuokaEnv(gym.Env): 17 | 18 | def __init__(self): 19 | 20 | log('[ENV] Initilializing environment') 21 | 22 | # Gym initialization 23 | # Actions - [gain_factor_l_hip_y, gain_factor_r_hip_y] 24 | # States - [torso_alpha, torso_beta, torso_gamma, d_torso_alpha, d_torso_beta, d_torso_gamma, torso_x, torso_y, d_torso_x, d_torso_y] 25 | 26 | # Set the max and min gain factor 27 | # The gain factor is multiplied with the joint gain 28 | self.gain_factor_max = 1.0 29 | self.gain_factor_min = 1.0 30 | 31 | # Initialize the gain factors 32 | self.gain_factor_l_hip_y = 1.0 33 | self.gain_factor_r_hip_y = 1.0 34 | 35 | # Initialize the action and observation spaces 36 | obs = np.array([np.inf] * 10) 37 | self.action_space = spaces.Box(low=np.array([0.75, 0.75]), high=np.array([1.0, 1.0])) 38 | self.observation_space = spaces.Box(-obs, obs) 39 | 40 | # Initilize the observation variables 41 | self.observation = None 42 | 43 | # Variable for the number of steps in the episode 44 | self.step_counter = 0 45 | 46 | # Set the oscillator thread 47 | # This will initialize the VREP scene and set the robot to the starting position 48 | self.oscillator_thread = Oscillator3Thread() 49 | 50 | # Start the oscillator thread 51 | # This will start the matsuoka walk 52 | self.oscillator_thread.start() 53 | 54 | def _self_observe(self): 55 | 56 | self.observation = self.oscillator_thread.self_observe() 57 | 58 | def _step(self, actions): 59 | 60 | # Set the actions 61 | self.oscillator_thread.self_action(actions) 62 | 63 | # Time for the actions to take effect 64 | time.sleep(1.0) 65 | 66 | # Make observation 67 | self._self_observe() 68 | 69 | # Calculate the reward 70 | # Since the robot starts at x=0,y=0 and faces the x-direction, the reward is calculated as -1.0*abs(y position) 71 | objpos = self.oscillator_thread.monitor_thread.objpos 72 | fallen = self.oscillator_thread.monitor_thread.fallen 73 | torso_orientation = self.oscillator_thread.monitor_thread.torso_euler_angles 74 | forward_x = objpos[0] 75 | deviation = objpos[1] 76 | torso_gamma = torso_orientation[2] 77 | 78 | reward = ALPHA*(-abs(deviation)) + BETA*forward_x + THETA*(-abs(torso_gamma)) 79 | 80 | log('[ENV] Deviation: {0} X-distance: {1} Torso-gamma: {2}'.format(deviation, forward_x, torso_gamma)) 81 | 82 | # Increment the step counter 83 | self.step_counter += 1 84 | 85 | # Large negative reward if the robot falls 86 | if fallen: 87 | reward -= 100.0 88 | 89 | return self.observation, reward, self.oscillator_thread.terminal, {} 90 | 91 | def _reset(self): 92 | 93 | log('[ENV] Resetting the environment') 94 | 95 | self.oscillator_thread.monitor_thread.stop() 96 | self.oscillator_thread.monitor_thread.join() 97 | # Close the VREP connection 98 | self.oscillator_thread.robot_handle.cleanup() 99 | self.oscillator_thread.stop() 100 | self.oscillator_thread.join() 101 | self.__init__() 102 | self._self_observe() # Set the observation 103 | return self.observation 104 | 105 | def _close(self): 106 | log('[ENV] Stopping the environment') 107 | self.oscillator_thread.monitor_thread.stop() 108 | self.oscillator_thread.monitor_thread.join() 109 | # Close the VREP connection 110 | self.oscillator_thread.robot_handle.cleanup() 111 | self.oscillator_thread.stop() 112 | self.oscillator_thread.join() 113 | -------------------------------------------------------------------------------- /matsuoka_walk/monitor.py: -------------------------------------------------------------------------------- 1 | """ 2 | Module for threaded classes 3 | """ 4 | 5 | import time 6 | from threading import Thread 7 | from pypot.vrep.io import VrepIO 8 | 9 | 10 | # import rospy 11 | # from std_msgs.msg import String 12 | # from bio_walk.ros import force_sensor_callback 13 | 14 | 15 | class RobotMonitorThread(Thread): 16 | """ 17 | Class for monitoring objects in VREP simulator. 18 | This class extends the Thread class and will run as in parallel to the main simulation. 19 | Only one monitoring thread is enough for monitoring a simulation. 20 | """ 21 | 22 | def __init__(self, portnum, objname, height_threshold): 23 | """ 24 | Initializes the RobotMonitorThread thread 25 | 26 | :param portnum: Port number on which the VREP remote API is listening on the server 27 | (different than the one used to run the main robot simulation) 28 | :param objname: Object name which is to be monitored 29 | :height_threshold: If the object's height is lower than this value, the robot is considered to have falen 30 | """ 31 | 32 | # Call init of super class 33 | Thread.__init__(self) 34 | 35 | # Create a VrepIO object using a different port number than the one used to run the main robot simulation 36 | # Make sure that the VREP remote api on the server is listening on this port number 37 | # Additional ports for listening can be set up on the server side by editing the file remoteApiConnections.txt 38 | self.vrepio_obj = VrepIO(vrep_port=portnum) 39 | 40 | # The object to be monitored 41 | self.objname = objname 42 | 43 | # Threshold for falling 44 | self.height_threshold = height_threshold 45 | 46 | # The position of the object 47 | self.objpos = None 48 | 49 | # The x,y,z coordinates of the position 50 | self.x = None 51 | self.y = None 52 | self.z = None 53 | 54 | # The orientation of the body 55 | self.torso_euler_angles = None 56 | 57 | # The starting time 58 | self.start_time = time.time() 59 | self.up_time = 0.0 60 | 61 | # The average height of the robot 62 | self.avg_z = 0.0 63 | 64 | # A list to store the height at each second 65 | self.z_list = list() 66 | 67 | # A flag which can stop the thread 68 | self.stop_flag = False 69 | 70 | # A flag to indicate if the robot has fallen 71 | self.fallen = False 72 | 73 | # Set up the ROS listener 74 | # rospy.init_node('nico_feet_force_listener', anonymous=True) 75 | # rospy.Subscriber("/nico_feet_forces", String, force_sensor_callback) 76 | 77 | def reset_timer(self): 78 | self.start_time = time.time() 79 | 80 | def run(self): 81 | """ 82 | The monitoring logic is implemented here. The self.objpos is updated at a preset frequency with the latest 83 | object positions. 84 | 85 | :return: None 86 | """ 87 | 88 | while not self.stop_flag: 89 | 90 | # Update the current position 91 | self.objpos = self.vrepio_obj.get_object_position(self.objname) 92 | self.x = self.objpos[0] 93 | self.y = self.objpos[1] 94 | self.z = self.objpos[2] 95 | 96 | self.torso_euler_angles = self.vrepio_obj.call_remote_api('simxGetObjectOrientation', 97 | self.vrepio_obj.get_object_handle( 98 | 'torso_11_respondable'), 99 | -1, 100 | # Orientation needed with respect to world frame 101 | streaming=True) 102 | 103 | # Update the time 104 | self.up_time = time.time() - self.start_time 105 | 106 | # Append the current height to the height list 107 | self.z_list.append(self.z) 108 | 109 | if self.z < self.height_threshold: 110 | # Set the flag which indicates that the robot has fallen 111 | self.fallen = True 112 | # Calculate the average height 113 | self.avg_z = sum(self.z_list) / float(len(self.z_list)) 114 | 115 | # Sleep 116 | time.sleep(0.1) 117 | 118 | def stop(self): 119 | """ 120 | Sets the flag which will stop the thread 121 | 122 | :return: None 123 | """ 124 | # Flag to stop the monitoring thread 125 | self.stop_flag = True 126 | 127 | # Close the monitoring vrep connection 128 | self.vrepio_obj.close() 129 | 130 | 131 | -------------------------------------------------------------------------------- /matsuoka_walk/monitor_1.py: -------------------------------------------------------------------------------- 1 | """ 2 | Modified monitor script with force sensing capability 3 | """ 4 | 5 | import time 6 | from threading import Thread 7 | from pypot.vrep.io import VrepIO 8 | 9 | # import rospy 10 | # from std_msgs.msg import String 11 | # from bio_walk.ros import force_sensor_callback 12 | 13 | 14 | class RobotMonitorThread(Thread): 15 | """ 16 | Class for monitoring objects in VREP simulator. 17 | This class extends the Thread class and will run as in parallel to the main simulation. 18 | Only one monitoring thread is enough for monitoring a simulation. 19 | """ 20 | 21 | def __init__(self, portnum, objname, height_threshold, force_threshold=10): 22 | """ 23 | Initializes the RobotMonitorThread thread 24 | 25 | :param portnum: Port number on which the VREP remote API is listening on the server 26 | (different than the one used to run the main robot simulation) 27 | :param objname: Object name which is to be monitored 28 | :height_threshold: If the object's height is lower than this value, the robot is considered to have falen 29 | """ 30 | 31 | # Call init of super class 32 | Thread.__init__(self) 33 | 34 | # Create a VrepIO object using a different port number than the one used to run the main robot simulation 35 | # Make sure that the VREP remote api on the server is listening on this port number 36 | # Additional ports for listening can be set up on the server side by editing the file remoteApiConnections.txt 37 | self.vrepio_obj = VrepIO(vrep_port=portnum) 38 | 39 | # The object to be monitored 40 | self.objname = objname 41 | 42 | # Threshold for falling 43 | self.height_threshold = height_threshold 44 | 45 | # Threshold for force sensor 46 | self.force_threshold = force_threshold 47 | 48 | # The position of the object 49 | self.objpos = None 50 | 51 | # Initialize the forces 52 | self.r1 = 0.0 53 | self.r2 = 0.0 54 | self.r3 = 0.0 55 | self.r4 = 0.0 56 | self.r_heel_current = 0.0 57 | self.r_heel_previous = 0.0 58 | 59 | # Flag for phase reset 60 | self.phase_reset = False 61 | 62 | 63 | # The x,y,z coordinates of the position 64 | self.x = None 65 | self.y = None 66 | self.z = None 67 | 68 | # The starting time 69 | self.start_time = time.time() 70 | self.up_time = 0.0 71 | 72 | # The average height of the robot 73 | self.avg_z = 0.0 74 | 75 | # A list to store the height at each second 76 | self.z_list = list() 77 | 78 | # A flag which can stop the thread 79 | self.stop_flag = False 80 | 81 | # A flag to indicate if the robot has fallen 82 | self.fallen = False 83 | 84 | # Set up the ROS listener 85 | # rospy.init_node('nico_feet_force_listener', anonymous=True) 86 | # rospy.Subscriber("/nico_feet_forces", String, force_sensor_callback) 87 | 88 | def reset_timer(self): 89 | self.start_time = time.time() 90 | 91 | def run(self): 92 | """ 93 | The monitoring logic is implemented here. The self.objpos is updated at a preset frequency with the latest 94 | object positions. 95 | 96 | :return: None 97 | """ 98 | 99 | while not self.stop_flag: 100 | 101 | # Update the current position 102 | self.objpos = self.vrepio_obj.get_object_position(self.objname) 103 | self.x = self.objpos[0] 104 | self.y = self.objpos[1] 105 | self.z = self.objpos[2] 106 | 107 | # Update the time 108 | self.up_time = time.time() - self.start_time 109 | 110 | # Append the current height to the height list 111 | self.z_list.append(self.z) 112 | 113 | if self.z < self.height_threshold: 114 | # Set the flag which indicates that the robot has fallen 115 | self.fallen = True 116 | # Calculate the average height 117 | self.avg_z = sum(self.z_list) / float(len(self.z_list)) 118 | 119 | # Update force sensor readings (first index signifies force vector and second index signifies z axis force) 120 | self.r1 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('right_sensor_1'), streaming=True)[1][2] 121 | self.r2 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('right_sensor_2'), streaming=True)[1][2] 122 | self.r3 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('right_sensor_3'), streaming=True)[1][2] 123 | self.r4 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('right_sensor_4'), streaming=True)[1][2] 124 | 125 | # Average the forces on the right heel 126 | self.r_heel_current = (self.r3 + self.r4)/2.0 127 | 128 | if self.r_heel_previous < self.force_threshold <= self.r_heel_current: 129 | self.phase_reset = True 130 | print('**** Resetting phase ****') 131 | print('prev_z_force: {0} curr_z_force: {1}'.format(self.r_heel_previous, self.r_heel_current)) 132 | else: 133 | self.phase_reset = False 134 | 135 | self.r_heel_previous = self.r_heel_current 136 | 137 | # Sleep 138 | time.sleep(0.1) 139 | 140 | def stop(self): 141 | """ 142 | Sets the flag which will stop the thread 143 | 144 | :return: None 145 | """ 146 | # Flag to stop the monitoring thread 147 | self.stop_flag = True 148 | 149 | # Close the monitoring vrep connection 150 | self.vrepio_obj.close() 151 | 152 | 153 | -------------------------------------------------------------------------------- /matsuoka_walk/ros.py: -------------------------------------------------------------------------------- 1 | """ 2 | Module for ROS functionality 3 | """ 4 | 5 | from bio_walk import Globals, log 6 | from std_msgs.msg import String 7 | import rospy 8 | import numpy as np 9 | import time 10 | 11 | # Force threshold for resetting phase 12 | force_threshold = 10.0 13 | 14 | global prev_z_force, curr_z_force 15 | prev_z_force = 0.0 16 | curr_z_force = 0.0 17 | 18 | 19 | def force_sensor_callback(data): 20 | 21 | global prev_z_force, curr_z_force 22 | 23 | # Retrieve the force plot_data as a string 24 | str_force_data = data.data 25 | 26 | # Convert into a numpy 2D array (8 rows, 3 columns) 27 | # Each row for 1 sensor (l1, l2, l3, l4, r1, r2, r3, r4) 28 | # Each row has x, y and z force values 29 | arr_force_data = np.fromstring(str_force_data.replace('{','').replace('}',''), sep=',').reshape((8,3)) 30 | 31 | # The left foot sensor values (force) 32 | l1 = arr_force_data[0] # toe 33 | l2 = arr_force_data[1] # toe 34 | l3 = arr_force_data[2] # heel 35 | l4 = arr_force_data[3] # heel 36 | 37 | # The right foot sensor values (force) 38 | r1 = arr_force_data[4] # toe 39 | r2 = arr_force_data[5] # toe 40 | r3 = arr_force_data[6] # heel 41 | r4 = arr_force_data[7] # heel 42 | 43 | # Average z forces for left foot 44 | l_toe_avg_z = (l1[2] + l2[2])/2.0 45 | l_heel_avg_z = (l3[2] + l4[2]) / 2.0 46 | l_avg_z = (l1[2] + l2[2] + l3[2] + l4[2])/4.0 47 | 48 | # Average z forces for right foot 49 | r_toe_avg_z = (r1[2] + r2[2]) / 2.0 50 | r_heel_avg_z = (r3[2] + r4[2]) / 2.0 51 | r_avg_z = (r1[2] + r2[2] + r3[2] + r4[2]) / 4.0 52 | 53 | # Reset the phase only when the force goes from below the threshold to above threshold 54 | curr_z_force = r_heel_avg_z 55 | 56 | if prev_z_force < force_threshold <= curr_z_force: 57 | # When r_heel_avg_z goes above force_threshold reset the phase 58 | # Sleep for 0.2s for the change to take effect 59 | Globals.pr_feedback = -1.0 60 | log('**** Resetting phase ****') 61 | log('prev_z_force: {0} curr_z_force: {1}'.format(prev_z_force, curr_z_force)) 62 | time.sleep(0.2) 63 | else: 64 | # After the reset, change the feedback back to 0.0 65 | Globals.pr_feedback = 0.0 66 | 67 | prev_z_force = curr_z_force 68 | 69 | 70 | def test_listener(): 71 | 72 | # In ROS, nodes are uniquely named. If two nodes with the same 73 | # node are launched, the previous one is kicked off. The 74 | # anonymous=True flag means that rospy will choose a unique 75 | # name for our 'listener' node so that multiple listeners can 76 | # run simultaneously. 77 | rospy.init_node('listener', anonymous=True) 78 | 79 | rospy.Subscriber("/nico_feet_forces", String, force_sensor_callback) 80 | 81 | # spin() simply keeps python from exiting until this node is stopped 82 | rospy.spin() 83 | 84 | if __name__ == '__main__': 85 | test_listener() 86 | -------------------------------------------------------------------------------- /matsuoka_walk/single.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from matsuoka_walk import oscillator_nw, Logger, log 4 | from matsuoka_walk.oscillator_2 import oscillator_nw as oscillator_nw2 5 | from matsuoka_walk.oscillator_3 import oscillator_nw as oscillator_nw3 6 | from matsuoka_walk.oscillator_3_eval import oscillator_nw as oscillator_nw3_1 7 | from matsuoka_walk.oscillator_3 import oscillator_nw as oscillator_nw3 8 | from matsuoka_walk.oscillator_5 import oscillator_nw as oscillator_nw5 9 | 10 | import numpy as np 11 | 12 | # Set the home directory 13 | home_dir = os.path.expanduser('~') 14 | 15 | # Set the logging variables 16 | # This also creates a new log file 17 | Logger(log_dir=os.path.join(home_dir, '.bio_walk/logs/'), log_flag=True) 18 | 19 | # 24.08.2017 Serial# 11 20 | chromosome_11 = [0.2809894424800271, 0.010641287858841346, 0.3536042565675742, 0.6866544967236521, 0.06227416763355498, 0.37395409236022054, 0.548276625368456, -0.22209817512550967, 0.2406521294673183, -0.14720850416137943, 0.42069906316742134] 21 | 22 | # Serial# 12 Open loop 23 | # Overall best chromosome after gen 21 24 | chromosome_12_best = [0.5884145884963005, 0.5369511850955603, 0.05524939367537715, 0.025252652932915343, 0.3493674052847861, 0.15003845298477264, 0.8168417842597174, -0.3021960037316898, 0.27767678286808256, -0.08093276016328882, 0.06397013154851627] 25 | chromosome_12_39 = [0.5884145884963005, 0.5546389061322409, 0.06503765398376404, 0.025252652932915343, 0.3493674052847861, 0.15003845298477264, 0.9355555608397872, -0.3818682418656224, 0.28272635930326756, -0.08093276016328882, 0.28736398799069907] 26 | # Gen 30 shows a good enough walk 27 | chromosome_12_30 = [0.5884145884963005, 0.5369511850955603, 0.09131337379133, 0.025252652932915343, 0.3493674052847861, 0.15003845298477264, 0.9377263350996107, -0.3099780890687065, 0.28272635930326756, -0.08259435820020733, 0.22178117552852405] 28 | chromosome_16 = [0.5906406229046759, 0.7183089896427572, 0.1341955351477535, 0.32810858432009504, 0.8491296082368245, 0.2160819134026336, 0.34564741214994854, -0.1099731914045623, 0.4243372736928702, -0.26634940163067344, 0.4380152785516842] 29 | #oscillator_nw2(chromosome_16, max_time=60.0) 30 | 31 | 32 | #chromosome_13 = [0.2622250290596007, 0.7366434517971292, 0.10563701968664051, 0.13846196212088752, 0.7746070841755859, 0.010347844024124576, 0.1500774281387774, -0.003881588885965126, 0.03256956870647948, -0.06417349167505343, 0.6361679562895832, 0.8598499950480331] 33 | 34 | 35 | # Angle feedback 36 | # Best ever after gen 25 37 | chromosome_14_best = [0.7461913734531209, 0.8422944031253159, 0.07043758116681641, 0.14236621222553963, 0.48893497409925746, 0.5980055418720059, 0.740811806645801, -0.11618361090424223, 0.492832184960149, -0.2949145038394889, 0.175450703085948, -0.3419733470484183] 38 | # More than 2m, has not fallen - dont use 39 | # chromosome_14_1 = [0.746191373453, 0.842294403125, 0.0704375811668, 0.0236283438094, 0.691052361025, 0.283076039513, 0.201727074382, -0.0571819786562, 0.0729859813752, -0.065040277261, 0.175450703086, -0.341973347048 ] 40 | # Gen 30 shows a good enough walk 41 | chromosome_14_30 = [0.746191373453, 0.842294403125, 0.0481025565348, 0.132100925761, 0.691052361025, 0.598005541872, 0.201727074382, -0.116183610904, 0.489361835504, -0.311722810344, 0.55840755813, -0.341973347048] 42 | 43 | # run from wtmpc23 (log_20170928_104617.txt) 44 | chromosome_17_18 = [0.25875216890878, 0.4261374577115516, 0.14424351823067832, 0.17757054603689745, 0.8075006509666804, 0.12160111156716906, 0.40977998188906734, -0.021616209300347883, 0.024680402602446694, -0.04799183022530174, 0.5084440214074175, 0.09969871880221692] 45 | chromosome_wtmpc23 = [0.2644022508682281, 0.4552126979693223, 0.012467129675918318, 0.03828988554695599, 0.7340750027962866, 0.26151421276068004, 0.9463092817659473, -0.0741996127978023, 0.1795014887697381, -0.15164263559891972, 0.4936867546872986, 1.0719839943269078] 46 | chromosome_20_best = [0.6450404705811301, 0.7693348395420826, 0.019270973206137753, 0.09411469737444257, 0.49928395481568916, 0.3631655506638598, 0.4917977003145482, -0.333420483018255, 0.46330629494181336, -0.21254793707792774, 0.01824073139337634, -0.17826436059862738] 47 | 48 | oscillator_nw3(chromosome_wtmpc23, max_time=60.0) 49 | 50 | # Testing if the gain change does turn the robot 51 | # test_iters = 100 52 | # avg_right = list() 53 | # for i in range(test_iters): 54 | # # Right leg has smaller stride, go right 55 | # y = oscillator_3_test_yaw(chromosome_14_best, max_time=20.0, gain_l=1.0, gain_r=0.7) 56 | # avg_right.append(y) 57 | # log('[Test right] iteration: {0} y_dev: {1}'.format(i, y)) 58 | # 59 | # avg_straight = list() 60 | # for j in range(test_iters): 61 | # # Go straight 62 | # y = oscillator_3_test_yaw(chromosome_14_best, max_time=20.0, gain_l=1.0, gain_r=1.0) 63 | # avg_straight.append(y) 64 | # log('[Test straight] iteration: {0} y_dev: {1}'.format(j, y)) 65 | # 66 | # avg_left = list() 67 | # for k in range(test_iters): 68 | # # Left leg has smaller stride, go left 69 | # y = oscillator_3_test_yaw(chromosome_14_best, max_time=20.0, gain_l=0.7, gain_r=1.0) 70 | # avg_left.append(y) 71 | # log('[Test left] iteration: {0} y_dev: {1}'.format(k, y)) 72 | # 73 | # log('[Test results] Left: {0}, Straight: {1}, Right: {2}'.format(np.mean(avg_left), np.mean(avg_straight), np.mean(avg_right))) 74 | 75 | # Phase reset 76 | # Best ever after gen 16 77 | # Best ever individual 78 | chromosome_15_best = [0.5146344913630376, 0.7455092402318735, -0.0070835220642929495, 0.13405758327500758, 0.6090967083734895, 0.6937935725821373, 0.21053517408377884, -0.3906102732254958, 0.4640728729649564, -0.23084688159919053, 0.7871274895040696] 79 | # Best individual after 44 generations - shows the best walk 80 | # chromosome_15_44 = [0.525773820572072, 0.7455092402318735, -0.0070835220642929495, 0.11122944383094965, 0.45329367561406486, 0.36757287747454864, 0.09138401154177767, -0.3906102732254958, 0.48870071798756914, -0.21725902225321198, 0.1259636974886932] 81 | # chromosome_15_35 = [0.5146344913630376, 0.7455092402318735, 0.013249201450675324, 0.11122944383094965, 0.45329367561406486, 0.36757287747454864, 0.09138401154177767, -0.3906102732254958, 0.48870071798756914, -0.22819402334346867, 0.11514607350697748] 82 | # Generation 30 shows as good a walk as generation 44 83 | chromosome_15_30 = [0.5146344913630376, 0.7455092402318735, 0.02567339112892708, 0.11122944383094965, 0.45329367561406486, 0.36757287747454864, 0.21053517408377884, -0.3906102732254958, 0.48870071798756914, -0.21330757509005024, 0.11514607350697748] 84 | #oscillator_nw5(chromosome_15_30, max_time=60.0) 85 | chromosome_28_best = [0.8597258814566455, 0.43299754018667147, 0.14367821852476442, 0.08404019333004167, 0.7634723943690848, 0.39147229949169776, 0.11048570137787744, -0.1408267335975779, 0.08740573084913256, -0.06380256717714489, 0.08800174238458613] 86 | #oscillator_nw(chromosome_11, max_time=60.0) 87 | 88 | #oscillator_nw3(chromosome_14_1, max_time=60.0) 89 | chromosome_18 = [0.23348735003280308, 0.5940097490118389, 0.046948415259721536, 0.29614276347710533, 0.5366182035136196, 0.045238888292921844, 0.7407980538549279, -0.21143743294519102, 0.27932024198484895, -0.12374820261024128, 0.937617707326727] 90 | chromosome_18_16 = [0.6444490761087627, 0.8714565517433243, 0.23366307517466375, 0.22910610853199953, 0.8336350815504616, 0.08141310496503874, 0.3221935902098477, -0.15500792195088986, 0.2439713677828791, -0.18259633151654048, 0.8477326981496035] 91 | #oscillator_nw5(chromosome_28_best, max_time=60.0) 92 | -------------------------------------------------------------------------------- /matsuoka_walk/slow_angle_test.py: -------------------------------------------------------------------------------- 1 | import os 2 | from matsuoka_walk.robots import Nico 3 | 4 | home_dir = os.path.expanduser('~') 5 | 6 | robot_handle = Nico(sync_sleep_time=0.1, 7 | motor_config=os.path.join(home_dir,'computing/repositories/hierarchical_bipedal_controller/motor_configs/nico_humanoid_full_v1.json'), 8 | vrep=True, 9 | vrep_host='127.0.0.1', 10 | vrep_port=19997, 11 | vrep_scene=os.path.join(home_dir,'computing/repositories/hierarchical_bipedal_controller/vrep_scenes/NICO-Simplified-July2017_standing.ttt') 12 | ) 13 | 14 | target_angles = {'l_shoulder_y': -0.6, 'r_shoulder_y':0.6} 15 | 16 | robot_handle.set_angles_slow(target_angles, 5.0) -------------------------------------------------------------------------------- /matsuoka_walk/static_deviation_test.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | 4 | from matsuoka_walk import Logger, log 5 | from matsuoka_walk.oscillator_3_test_yaw import oscillator_nw as oscillator_3_test_yaw 6 | 7 | # Set the home directory 8 | home_dir = os.path.expanduser('~') 9 | 10 | # Set the logging variables 11 | # This also creates a new log file 12 | Logger(log_dir=os.path.join(home_dir, '.bio_walk/logs/'), log_flag=True) 13 | 14 | LOWEST_POSSIBLE_GAIN = 0.4 15 | 16 | log('[STATIC TEST] LOWEST_POSSIBLE_GAIN: {}'.format(LOWEST_POSSIBLE_GAIN)) 17 | 18 | wtmpc23_run3_best30 = [0.3178385532762875, 0.3777451259604342, 0.023411599863716586, 0.013217696615302215, 0.4566963469455763, 0.20194162123716233, 0.3309010463046798, -0.05187677829896087, 0.09633745660574622, -0.11559976203529859, 0.4814311312157089, 1.5364038978521224] 19 | asus_run1_bestall = [0.7461913734531209, 0.8422944031253159, 0.07043758116681641, 0.14236621222553963, 0.48893497409925746, 0.5980055418720059, 0.740811806645801, -0.11618361090424223, 0.492832184960149, -0.2949145038394889, 0.175450703085948, -0.3419733470484183] 20 | 21 | best_chromosome = asus_run1_bestall 22 | 23 | # Testing if the gain change does turn the robot 24 | test_iters = 1 25 | #avg_right = list() 26 | #for i in range(test_iters): 27 | # # Right leg has smaller stride, go right 28 | # (x, y) = oscillator_3_test_yaw(best_chromosome, max_time=20.0, gain_l=1.0, gain_r=LOWEST_POSSIBLE_GAIN) 29 | # avg_right.append(y) 30 | # log('[STATIC TEST right] iteration: {0} Distance: {1} Deviation: {2}'.format(i, x, y)) 31 | 32 | avg_straight = list() 33 | for j in range(test_iters): 34 | # Go straight 35 | (x, y, gamma) = oscillator_3_test_yaw(best_chromosome, max_time=40.0, gain_l=1.0, gain_r=0.1) 36 | avg_straight.append(y) 37 | log('[STATIC TEST straight] iteration: {0} Distance: {1} Deviation: {2} Torso-gamma: {3}'.format(j, x, y, gamma)) 38 | 39 | #avg_left = list() 40 | #for k in range(test_iters): 41 | # # Left leg has smaller stride, go left 42 | # (x, y) = oscillator_3_test_yaw(best_chromosome, max_time=20.0, gain_l=LOWEST_POSSIBLE_GAIN, gain_r=1.0) 43 | # avg_left.append(y) 44 | # log('[STATIC TEST left] iteration: {0} Distance: {1} Deviation: {2}'.format(k, x, y)) 45 | 46 | #log('[Test results] Left: {0}, Straight: {1}, Right: {2}'.format(np.mean(avg_left), np.mean(avg_straight), np.mean(avg_right))) 47 | 48 | -------------------------------------------------------------------------------- /plot_scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sayantanauddy/hierarchical_bipedal_controller/ef1dfbd7f8e3ced3c574d15bcddca634e4d9de6c/plot_scripts/__init__.py -------------------------------------------------------------------------------- /plot_scripts/analyze_log.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from collections import Counter 4 | 5 | # Fetch the individual position vectors from the log file 6 | os.system("grep 'indiv_position' log_20170813_001048.txt | sed -r 's/.{42}//' | sed 's/[\[]//g'| sed 's/\]//g' > positions.txt") 7 | 8 | # Read into an np array 9 | with open('positions.txt') as file: 10 | array2d = [[float(digit) for digit in line.split(',')] for line in file] 11 | array2d = np.array(array2d) 12 | 13 | # Read the first 50 rows 14 | array2d_1 = array2d[0:50,:] 15 | 16 | # Count the frequency of unique values in each column over each iteration 17 | start=0 18 | end=50 19 | for i in range(200): 20 | if i%50==0: 21 | array2d_1 = array2d[start:end,:] 22 | for j in range(array2d_1.shape[1]): 23 | count = Counter(array2d_1[:, j]) 24 | print 'Frequency of column {0} over iteration {1}'.format(j,i+1) 25 | print count.most_common(3) 26 | print '========================' 27 | start += 50 28 | end += 50 29 | 30 | print'' 31 | 32 | # Count the frequency of unique values in each column over all iterations together 33 | print 'Frequency over all iterations' 34 | for i in range(array2d.shape[1]): 35 | count = Counter(array2d[:, i]) 36 | print count.most_common(3) 37 | 38 | 39 | -------------------------------------------------------------------------------- /plot_scripts/benchmark_fitness_function_surface_plots.py: -------------------------------------------------------------------------------- 1 | from mpl_toolkits.mplot3d import Axes3D 2 | from matplotlib import cm 3 | import matplotlib.pyplot as plt 4 | from matplotlib.colors import LogNorm 5 | 6 | import os 7 | 8 | try: 9 | import numpy as np 10 | except: 11 | exit() 12 | 13 | from deap import benchmarks 14 | 15 | # Set the home directory 16 | home_dir = os.path.expanduser('~') 17 | 18 | # Set the directory for saving plots 19 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 20 | 21 | 22 | def griewank_arg0(sol): 23 | return benchmarks.griewank(sol)[0] 24 | 25 | # Plot for Griewank 26 | 27 | fig = plt.figure(1) 28 | ax = Axes3D(fig, azim = -29, elev = 40) 29 | # ax = Axes3D(fig) 30 | X = np.arange(-50, 50, 0.5) 31 | Y = np.arange(-50, 50, 0.5) 32 | X, Y = np.meshgrid(X, Y) 33 | Z = np.fromiter(map(griewank_arg0, zip(X.flat,Y.flat)), dtype=np.float, count=X.shape[0]*X.shape[1]).reshape(X.shape) 34 | 35 | ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.jet, linewidth=0.2) 36 | 37 | plt.xlabel("x") 38 | plt.ylabel("y") 39 | plt.legend() 40 | plt.savefig(os.path.join(plot_dir, 'griewank.eps')) 41 | 42 | 43 | # Plot for Rastigrin 44 | 45 | def rastrigin_arg0(sol): 46 | return benchmarks.rastrigin(sol)[0] 47 | 48 | 49 | fig = plt.figure(2) 50 | ax = Axes3D(fig, azim=-29, elev=50) 51 | X = np.arange(-5, 5, 0.1) 52 | Y = np.arange(-5, 5, 0.1) 53 | X, Y = np.meshgrid(X, Y) 54 | Z = np.fromiter(map(rastrigin_arg0, zip(X.flat, Y.flat)), dtype=np.float, count=X.shape[0] * X.shape[1]).reshape( 55 | X.shape) 56 | 57 | ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.jet, linewidth=0.2) 58 | 59 | plt.xlabel("x") 60 | plt.ylabel("y") 61 | 62 | plt.savefig(os.path.join(plot_dir, 'rastrigin.eps')) 63 | 64 | 65 | # Plot for Schwefel 66 | 67 | def schwefel_arg0(sol): 68 | return benchmarks.schwefel(sol)[0] 69 | 70 | fig = plt.figure(3) 71 | # ax = Axes3D(fig, azim = -29, elev = 50) 72 | ax = Axes3D(fig) 73 | X = np.arange(-500, 500, 10) 74 | Y = np.arange(-500, 500, 10) 75 | X, Y = np.meshgrid(X, Y) 76 | Z = np.fromiter(map(schwefel_arg0, zip(X.flat,Y.flat)), dtype=np.float, count=X.shape[0]*X.shape[1]).reshape(X.shape) 77 | 78 | ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.jet, linewidth=0.2) 79 | 80 | plt.xlabel("x") 81 | plt.ylabel("y") 82 | 83 | plt.savefig(os.path.join(plot_dir, 'schwefel.eps')) 84 | 85 | 86 | # Plot for Shekel 87 | 88 | A = [[0.5, 0.5], [0.25, 0.25], [0.25, 0.75], [0.75, 0.25], [0.75, 0.75]] 89 | C = [0.002, 0.005, 0.005, 0.005, 0.005] 90 | 91 | 92 | def shekel_arg0(sol): 93 | return benchmarks.shekel(sol, A, C)[0] 94 | 95 | 96 | fig = plt.figure(4) 97 | # ax = Axes3D(fig, azim = -29, elev = 50) 98 | ax = Axes3D(fig) 99 | X = np.arange(0, 1, 0.01) 100 | Y = np.arange(0, 1, 0.01) 101 | X, Y = np.meshgrid(X, Y) 102 | Z = np.fromiter(map(shekel_arg0, zip(X.flat, Y.flat)), dtype=np.float, count=X.shape[0] * X.shape[1]).reshape(X.shape) 103 | 104 | ax.plot_surface(X, Y, Z, rstride=1, cstride=1, norm=LogNorm(), cmap=cm.jet, linewidth=0.2) 105 | 106 | plt.xlabel("x") 107 | plt.ylabel("y") 108 | 109 | plt.savefig(os.path.join(plot_dir, 'shekel.eps')) 110 | -------------------------------------------------------------------------------- /plot_scripts/ga_plot_paper.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import pandas as pd 5 | import os 6 | import matplotlib 7 | 8 | # For avoiding type-3 fonts 9 | matplotlib.rcParams['pdf.fonttype'] = 42 10 | matplotlib.rcParams['ps.fonttype'] = 42 11 | 12 | # Set the home directory 13 | home_dir = os.path.expanduser('~') 14 | 15 | # Set the directory for saving plots 16 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 17 | 18 | # Directory for saving plot data files 19 | plot_data_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plot_data') 20 | 21 | # Set the directory where logs are stored 22 | log_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/logs/genetic_algorithm_logs') 23 | 24 | 25 | def gen_fitness_plot(gen, max_f, avg_f, max_d, plot_name, grid=False): 26 | 27 | # Figure 1 for open loop 28 | fig, ax = plt.subplots(1,2,figsize=(20,5)) 29 | 30 | # Change the font size used in the plots 31 | font_size = 36 32 | plt.rcParams.update({'font.size': font_size}) 33 | 34 | # Set the line width 35 | linewidth=3.5 36 | 37 | # Counters for row and columns 38 | r,c = 1,1 39 | 40 | # Set the x-axis limits 41 | ax[0].set_xlim([1, 30]) 42 | ax[0].set_xlabel('Generation', fontsize=font_size) 43 | ax[1].set_xlim([1, 30]) 44 | ax[1].set_xlabel('Generation', fontsize=font_size) 45 | 46 | # Set the y-axis limits 47 | ax[0].set_ylim([0, 14.8]) 48 | ax[0].set_ylabel('Fitness', fontsize=font_size, horizontalalignment='center') 49 | 50 | ax[1].set_ylim([0, 4.8]) 51 | ax[1].set_ylabel('Max. Distance (m)', fontsize=font_size, horizontalalignment='center') 52 | 53 | # Set the axis labels (since x-axis is shared, only the last subplot has an xlabel) 54 | 55 | ax[0].yaxis.set_ticks_position('both') 56 | ax[0].axes.yaxis.set_ticklabels([0,'',4,'',8,'',12,'']) 57 | ax[0].tick_params(axis='y', pad=15) 58 | ax[0].tick_params(axis='x', pad=15) 59 | 60 | ax[1].yaxis.set_ticks_position('both') 61 | #ax2.axes.yaxis.set_ticklabels([0, 1, 2, 3, 4]) 62 | ax[1].tick_params(axis='y', pad=15) 63 | ax[1].tick_params(axis='x', pad=15) 64 | 65 | # First subplot contains max and avg fitness for run 1 66 | l0, = ax[0].plot(gen, max_f, color='blue', linewidth=linewidth, label='Max. Fitness') 67 | l1, = ax[0].plot(gen, avg_f, color='red', linewidth=linewidth, label='Avg. Fitness') 68 | leg = ax[0].legend(loc='lower right', fontsize=font_size-4) 69 | leg.get_frame().set_alpha(0.5) 70 | 71 | l2, = ax[1].plot(gen, max_d, color='green', linewidth=linewidth, label='Max. Distance') 72 | if grid: 73 | ax[0].grid(color='#222222', linestyle='dotted', linewidth=1) 74 | ax[1].grid(color='#222222', linestyle='dotted', linewidth=1) 75 | 76 | plt.subplots_adjust(wspace=0.25) 77 | #plt.legend(handles=[l0, l1, l2], bbox_to_anchor=(1.025, 1.4), loc='upper right', ncol=3, fontsize=font_size-4) 78 | plt.savefig(os.path.join(plot_dir, plot_name), bbox_inches='tight') 79 | plt.show() 80 | 81 | wtmpc_anglefeedback_run1 = {'data_fitness': 'ga_fitness_log_20171002_182720.csv', 'data_max_dist': 'ga_max_dist_log_20171002_182720.csv'} 82 | wtmpc_anglefeedback_run2 = {'data_fitness': 'ga_fitness_log_20171004_173049.csv', 'data_max_dist': 'ga_max_dist_log_20171004_173049.csv'} 83 | wtmpc_anglefeedback_run3 = {'data_fitness': 'ga_fitness_log_20171006_145448.csv', 'data_max_dist': 'ga_max_dist_log_20171006_145448.csv'} 84 | 85 | gen, min_f, max_f, avg_f, std_f = np.loadtxt(os.path.join(plot_data_dir, wtmpc_anglefeedback_run3['data_fitness']), delimiter=',', unpack=True, skiprows=1) 86 | gen, max_d = np.loadtxt(os.path.join(plot_data_dir, wtmpc_anglefeedback_run3['data_max_dist']), delimiter=',', unpack=True, skiprows=1) 87 | 88 | gen_fitness_plot(gen, max_f, avg_f, max_d, plot_name='ga_plot_paper.eps', grid=True) 89 | -------------------------------------------------------------------------------- /plot_scripts/gait_eval_param_correlation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import pandas as pd 5 | import os 6 | import seaborn as sns 7 | 8 | # Set the home directory 9 | home_dir = os.path.expanduser('~') 10 | 11 | # Set the directory for saving plots 12 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 13 | 14 | # Directory for saving plot data files 15 | plot_data_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plot_data') 16 | 17 | # Set the directory where logs are stored 18 | log_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/logs/reinforcement_learning_logs') 19 | 20 | global fignum 21 | fignum=1 22 | 23 | def create_corrmat_from_csv(datafile_fitness, datafile_x_dist, 24 | datafile_y_dev, datafile_foot, 25 | datafile_up, datafile_alpha, 26 | datafile_beta, datafile_gamma, 27 | plot_title=None, figsize=(10,10)): 28 | 29 | global fignum 30 | 31 | #plot_file = os.path.join(plot_dir,datafile.replace('.csv', '.svg')) 32 | colnames = ['OL1', 'OL2', 'OL3', 'AF1', 'AF2', 'AF3', 'PR1', 'PR2', 'PR3'] 33 | 34 | df_fitness = pd.read_csv(os.path.join(plot_data_dir, datafile_fitness), delimiter=',', names=colnames, header=0) 35 | df_x_dist = pd.read_csv(os.path.join(plot_data_dir, datafile_x_dist), delimiter=',', names=colnames, header=0) 36 | df_y_dev = pd.read_csv(os.path.join(plot_data_dir, datafile_y_dev), delimiter=',', names=colnames, header=0) 37 | df_foot = pd.read_csv(os.path.join(plot_data_dir, datafile_foot), delimiter=',', names=colnames, header=0) 38 | df_up = pd.read_csv(os.path.join(plot_data_dir, datafile_up), delimiter=',', names=colnames, header=0) 39 | df_alpha = pd.read_csv(os.path.join(plot_data_dir, datafile_alpha), delimiter=',', names=colnames, header=0) 40 | df_beta = pd.read_csv(os.path.join(plot_data_dir, datafile_beta), delimiter=',', names=colnames, header=0) 41 | df_gamma = pd.read_csv(os.path.join(plot_data_dir, datafile_gamma), delimiter=',', names=colnames, header=0) 42 | 43 | # Concatenate columns 44 | df_ol1 = pd.concat([df_fitness['OL1'],df_x_dist['OL1'],df_y_dev['OL1'],df_foot['OL1'],df_up['OL1'],df_alpha['OL1'],df_beta['OL1'],df_gamma['OL1'],], axis=1) 45 | df_ol2 = pd.concat([df_fitness['OL2'],df_x_dist['OL2'],df_y_dev['OL2'],df_foot['OL2'],df_up['OL2'],df_alpha['OL2'],df_beta['OL2'],df_gamma['OL2'],], axis=1) 46 | df_ol3 = pd.concat([df_fitness['OL3'],df_x_dist['OL3'],df_y_dev['OL3'],df_foot['OL3'],df_up['OL3'],df_alpha['OL3'],df_beta['OL3'],df_gamma['OL3'],], axis=1) 47 | 48 | df_af1 = pd.concat([df_fitness['AF1'], df_x_dist['AF1'], df_y_dev['AF1'], df_foot['AF1'], df_up['AF1'], df_alpha['AF1'],df_beta['AF1'], df_gamma['AF1'], ], axis=1) 49 | df_af2 = pd.concat([df_fitness['AF2'], df_x_dist['AF2'], df_y_dev['AF2'], df_foot['AF2'], df_up['AF2'], df_alpha['AF2'],df_beta['AF2'], df_gamma['AF2'], ], axis=1) 50 | df_af3 = pd.concat([df_fitness['AF3'], df_x_dist['AF3'], df_y_dev['AF3'], df_foot['AF3'], df_up['AF3'], df_alpha['AF3'],df_beta['AF3'], df_gamma['AF3'], ], axis=1) 51 | 52 | df_pr1 = pd.concat([df_fitness['PR1'], df_x_dist['PR1'], df_y_dev['PR1'], df_foot['PR1'], df_up['PR1'], df_alpha['PR1'],df_beta['PR1'], df_gamma['PR1'], ], axis=1) 53 | df_pr2 = pd.concat([df_fitness['PR2'], df_x_dist['PR2'], df_y_dev['PR2'], df_foot['PR2'], df_up['PR2'], df_alpha['PR2'],df_beta['PR2'], df_gamma['PR2'], ], axis=1) 54 | df_pr3 = pd.concat([df_fitness['PR3'], df_x_dist['PR3'], df_y_dev['PR3'], df_foot['PR3'], df_up['PR3'], df_alpha['PR3'],df_beta['PR3'], df_gamma['PR3'], ], axis=1) 55 | 56 | labels = [r'$f$', r'$distance_x$', r'$dev^{abs}_{y}$', r'$stride^{avg}$', r'$t_{up}$', r'$torso_\alpha^{var}$', r'$torso_\beta^{var}$', r'$torso_\gamma^{var}$'] 57 | 58 | figsize=(17,5) 59 | cmap_name = 'RdYlBu' # viridis, RdYlBu 60 | 61 | fig, ax = plt.subplots(nrows=1, ncols=3, sharex=False, sharey=True, figsize=figsize) 62 | # Row for open loop 63 | im = ax[0].matshow(df_ol1.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 64 | ax[0].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 65 | ax[0].set_yticklabels([''] + labels, fontsize=20) 66 | ax[0].autoscale(False) 67 | ax[1].matshow(df_ol2.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 68 | ax[1].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 69 | ax[1].autoscale(False) 70 | ax[1].set_yticklabels([''] + labels, fontsize=20) 71 | ax[2].matshow(df_ol3.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 72 | ax[2].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 73 | ax[2].autoscale(False) 74 | ax[2].set_yticklabels([''] + labels, fontsize=20) 75 | fig.subplots_adjust(right=0.8) 76 | cbar_ax = fig.add_axes([0.82, 0.1, 0.02, 0.8]) 77 | fig.colorbar(im, cax=cbar_ax) 78 | #for a in ax: 79 | # for tick in a.get_xticklabels(): 80 | # tick.set_rotation(90) 81 | plt.axis('on', fontsize=22) 82 | plt.xticks(ha='left') 83 | plt.subplots_adjust(wspace=0.03, hspace=5) 84 | plt.savefig(os.path.join(plot_dir, 'corr_OL.svg'), bbox_inches='tight') # Change the font size used in the plots 85 | 86 | 87 | fig, ax = plt.subplots(nrows=1, ncols=3, sharex=False, sharey=True, figsize=figsize) 88 | # Row for angle feedback 89 | im=ax[0].matshow(df_af1.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 90 | ax[0].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 91 | ax[0].set_yticklabels([''] + labels, fontsize=20) 92 | ax[1].matshow(df_af2.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 93 | ax[1].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 94 | ax[1].set_yticklabels([''] + labels, fontsize=20) 95 | ax[2].matshow(df_af3.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 96 | ax[2].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 97 | ax[2].set_yticklabels([''] + labels, fontsize=20) 98 | fig.subplots_adjust(right=0.8) 99 | cbar_ax = fig.add_axes([0.82, 0.1, 0.02, 0.8]) 100 | fig.colorbar(im, cax=cbar_ax) 101 | #for a in ax: 102 | # for tick in a.get_xticklabels(): 103 | # tick.set_rotation(90) 104 | #plt.axis('off') 105 | plt.axis('on', fontsize=22) 106 | plt.subplots_adjust(wspace=0.03, hspace=5) 107 | plt.savefig(os.path.join(plot_dir, 'corr_AF.svg'), bbox_inches='tight') # Change the font size used in the plots 108 | 109 | fig, ax = plt.subplots(nrows=1, ncols=3, sharex=False, sharey=True, figsize=figsize) 110 | # Row for phase reset 111 | im=ax[0].matshow(df_pr1.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 112 | ax[0].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 113 | ax[0].set_yticklabels([''] + labels, fontsize=20) 114 | ax[1].matshow(df_pr2.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 115 | ax[1].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 116 | ax[1].set_yticklabels([''] + labels, fontsize=20) 117 | ax[2].matshow(df_pr3.corr(), vmin=-1, vmax=1, cmap=plt.get_cmap(cmap_name)) 118 | ax[2].set_xticklabels([''] + labels, fontsize=20, rotation=45, ha='left') 119 | ax[2].set_yticklabels([''] + labels, fontsize=20) 120 | fig.subplots_adjust(right=0.8) 121 | cbar_ax = fig.add_axes([0.82, 0.1, 0.02, 0.8]) 122 | fig.colorbar(im, cax=cbar_ax) 123 | #for a in ax: 124 | # for tick in a.get_xticklabels(): 125 | # tick.set_rotation(90) 126 | plt.axis('on', fontsize=22) 127 | plt.xticks(ha='left') 128 | plt.subplots_adjust(wspace=0.03, hspace=5) 129 | plt.savefig(os.path.join(plot_dir, 'corr_PR.svg'), bbox_inches='tight') # Change the font size used in the plots 130 | 131 | create_corrmat_from_csv(datafile_fitness='wtmpc_gait_eval_fitness.csv', datafile_x_dist='wtmpc_gait_eval_x_dist.csv', 132 | datafile_y_dev='wtmpc_gait_eval_abs_y_dev.csv', datafile_foot='wtmpc_gait_eval_avg_footstep.csv', 133 | datafile_up='wtmpc_gait_eval_up_time.csv', datafile_alpha='wtmpc_gait_eval_var_alpha.csv', 134 | datafile_beta='wtmpc_gait_eval_var_beta.csv', datafile_gamma='wtmpc_gait_eval_var_gamma.csv', 135 | plot_title=None, figsize=(10,10)) -------------------------------------------------------------------------------- /plot_scripts/genetic_algorithm_plots.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import pandas as pd 5 | import os 6 | 7 | # Machine 1: ASUS 8 | # Machine 2: wtmpc 9 | ga_logs = [{'machine': '1', 'feedback': 'Open Loop', 'run': 1, 'log': 'log_20170830_172324.txt'}, 10 | {'machine': '1', 'feedback': 'Open Loop', 'run': 2, 'log': 'log_20171027_225729.txt'}, 11 | {'machine': '1', 'feedback': 'Open Loop', 'run': 3, 'log': 'log_20171030_033117.txt'}, 12 | 13 | {'machine': '1', 'feedback': 'Angle feedback', 'run': 1, 'log': 'log_20170908_214752.txt'}, 14 | {'machine': '1', 'feedback': 'Angle feedback', 'run': 2, 'log': 'log_20170929_131608.txt'}, 15 | {'machine': '1', 'feedback': 'Angle feedback', 'run': 3, 'log': 'log_20171009_101803.txt'}, 16 | 17 | {'machine': '1', 'feedback': 'Phase Reset', 'run': 1, 'log': 'log_20170921_110728.txt'}, 18 | {'machine': '1', 'feedback': 'Phase Reset', 'run': 2, 'log': 'log_20171001_170505.txt'}, 19 | {'machine': '1', 'feedback': 'Phase Reset', 'run': 3, 'log': 'log_20171012_012106.txt'}, 20 | 21 | {'machine': '2', 'feedback': 'Open Loop', 'run': 1, 'log': 'log_20171002_113858.txt'}, 22 | {'machine': '2', 'feedback': 'Open Loop', 'run': 2, 'log': 'log_20171004_094431.txt'}, 23 | {'machine': '2', 'feedback': 'Open Loop', 'run': 3, 'log': 'log_20171006_074055.txt'}, 24 | 25 | {'machine': '2', 'feedback': 'Angle feedback', 'run': 1, 'log': 'log_20171002_182720.txt'}, 26 | {'machine': '2', 'feedback': 'Angle feedback', 'run': 2, 'log': 'log_20171004_173049.txt'}, 27 | {'machine': '2', 'feedback': 'Angle feedback', 'run': 3, 'log': 'log_20171006_145448.txt'}, 28 | 29 | {'machine': '2', 'feedback': 'Phase Reset', 'run': 1, 'log': 'log_20171002_120630.txt'}, 30 | {'machine': '2', 'feedback': 'Phase Reset', 'run': 2, 'log': 'log_20171004_132604.txt'}, 31 | {'machine': '2', 'feedback': 'Phase Reset', 'run': 3, 'log': 'log_20171006_140001.txt'}] 32 | 33 | # Set the home directory 34 | home_dir = os.path.expanduser('~') 35 | 36 | # Set the directory for saving plots 37 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 38 | 39 | # Directory for saving plot data files 40 | plot_data_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plot_data') 41 | 42 | # Set the directory where logs are stored 43 | log_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/logs/genetic_algorithm_logs') 44 | 45 | # For each log file there will be 2 plots (2 data files) 46 | # 1. Plot of fitness (max, min, avg, std) vs generations 47 | # 2. Plot of best distance vs generations 48 | 49 | # So for 18 log files there will be 36 plots and 36 data files 50 | 51 | 52 | def generate_fitness_datafile(log_file_path): 53 | 54 | # Read the Min fitness for each generation 55 | min_fitness_search_string = 'Min' 56 | max_fitness_search_string = 'Max' 57 | avg_fitness_search_string = 'Avg' 58 | std_fitness_search_string = 'Std' 59 | 60 | min_list = list() 61 | max_list = list() 62 | avg_list = list() 63 | std_list = list() 64 | 65 | lines = open(log_file_path, "r").read().splitlines() 66 | 67 | for i, line in enumerate(lines): 68 | 69 | # Initialize the values 70 | min_fitness, max_fitness, avg_fitness, std_fitness = 0.0, 0.0, 0.0, 0.0 71 | 72 | if min_fitness_search_string in line: 73 | min_fitness = line.split(' ')[4] 74 | 75 | if max_fitness_search_string in line: 76 | max_fitness = line.split(' ')[4] 77 | 78 | if avg_fitness_search_string in line: 79 | avg_fitness = line.split(' ')[4] 80 | 81 | if std_fitness_search_string in line: 82 | std_fitness = line.split(' ')[4] 83 | 84 | # Append to the list if the values have been set 85 | if min_fitness > 0.0: 86 | min_list.append(min_fitness) 87 | 88 | if max_fitness > 0.0: 89 | max_list.append(max_fitness) 90 | 91 | if avg_fitness > 0.0: 92 | avg_list.append(avg_fitness) 93 | 94 | if std_fitness > 0.0: 95 | std_list.append(std_fitness) 96 | 97 | # Only process till the end of 30 generations 98 | if 'End of generation 30' in line: 99 | break 100 | 101 | # At this point file_list will have 30 values of min_fitness, max_fitness, avg_fitness, std_fitness 102 | # Create a list for generation numbers 103 | gen_list = range(1,31) 104 | 105 | # Crate data frames by using the 1D lists and reshaping them 106 | df_gen = pd.DataFrame(np.array(gen_list).reshape(30, 1), columns=['Generation']) 107 | df_min = pd.DataFrame(np.array(min_list).reshape(30, 1), columns=['Min_fitness']) 108 | df_max = pd.DataFrame(np.array(max_list).reshape(30, 1), columns=['Max_fitness']) 109 | df_avg = pd.DataFrame(np.array(avg_list).reshape(30, 1), columns=['Avg_fitness']) 110 | df_std = pd.DataFrame(np.array(std_list).reshape(30, 1), columns=['Std_fitness']) 111 | 112 | df_fitness = pd.concat([df_gen, df_min, df_max, df_avg, df_std], axis=1) 113 | 114 | # Derive the name of the data file from the log file 115 | fitness_data_file_path = os.path.join(plot_data_dir, 'ga_fitness_' + os.path.basename(log_file_path)).replace('.txt', '.csv') 116 | 117 | # Store the dataframe as a csv in the data folder 118 | df_fitness.to_csv(fitness_data_file_path, sep=',', index=False) 119 | 120 | # Now using the max fitness value in each generation, find the distance walked by that chromosome 121 | max_dist_list = list() 122 | curr_minus_line = '' 123 | curr_line = '' 124 | j = 2 125 | 126 | for max_fit in max_list: 127 | for i, line in enumerate(lines): 128 | if 'fitness: ' +str(max_fit) in line: 129 | curr_line = line 130 | for k in range(15): 131 | if 'end_x' in lines[i - k]: 132 | j=k 133 | curr_minus_line = lines[i - j] 134 | max_dist = curr_minus_line.split(' ')[3].split('=')[1].replace(',', '') 135 | max_dist_list.append(max_dist) 136 | 137 | # Only process till the end of generation 30 138 | if 'End of generation 30' in curr_line: 139 | break 140 | 141 | # Create a dataframe for max distance 142 | df_max_dist = pd.DataFrame(np.array(max_dist_list).reshape(30, 1), columns=['Max_distance']) 143 | df_max_dist = pd.concat([df_gen, df_max_dist], axis=1) 144 | 145 | # Derive the name of the data file from the log file 146 | max_dist_data_file_path = os.path.join(plot_data_dir, 'ga_max_dist_' + os.path.basename(log_file_path)).replace('.txt', '.csv') 147 | 148 | # Store the dataframe as a csv in the data folder 149 | df_max_dist.to_csv(max_dist_data_file_path, sep=',', index=False) 150 | 151 | return fitness_data_file_path, max_dist_data_file_path 152 | 153 | 154 | def generate_fitness_plot(datafile_path, machine, feedback, run): 155 | 156 | # Change the font size used in the plots 157 | font_size = 12 158 | plt.rcParams.update({'font.size': font_size}) 159 | 160 | # Read the datafile 161 | gen, min_f, max_f, avg_f, std_f = np.loadtxt(datafile_path, delimiter=',', unpack=True, skiprows=1) 162 | 163 | plt.figure() 164 | plt.xlabel('Generations') 165 | plt.ylabel('Fitness Value') 166 | plt.xlim(1, 30) 167 | plt.ylim(-5, 25) 168 | reward_plot_title = 'Fitness vs generations | Machine: {0} | Type: {1} | Run: {2}'.format(machine, feedback, run) 169 | plt.title(reward_plot_title, fontsize=font_size) 170 | plt.plot(gen, min_f, color='red', linewidth=2.0, label='Minimum Fitness') 171 | plt.plot(gen, max_f, color='blue', linewidth=2.0, label='Maximum Fitness') 172 | plt.plot(gen, avg_f, color='green', linewidth=2.0, label='Average Fitness') 173 | plt.plot(gen, std_f, color='orange', linewidth=2.0, label='Std. Fitness') 174 | plt.legend(prop={'size': font_size}) 175 | 176 | plot_file_path = os.path.basename(datafile_path).replace('.csv', '') + '.png' 177 | plt.axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 178 | plt.savefig(os.path.join(plot_dir, plot_file_path)) 179 | plt.close() 180 | plt.gcf().clear() 181 | 182 | return plot_file_path 183 | 184 | 185 | def generate_max_dist_plot(datafile_path, machine, feedback, run): 186 | # Change the font size used in the plots 187 | font_size = 12 188 | plt.rcParams.update({'font.size': font_size}) 189 | 190 | # Read the datafile 191 | gen, max_dist = np.loadtxt(datafile_path, delimiter=',', unpack=True, skiprows=1) 192 | 193 | plt.figure() 194 | plt.xlabel('Generations') 195 | plt.ylabel('Max Distance (m)') 196 | plt.xlim(1, 30) 197 | plt.ylim(-1, 5) 198 | reward_plot_title = 'Max distance vs generations | Machine: {0} | Type: {1} | Run: {2}'.format(machine, feedback, run) 199 | plt.title(reward_plot_title, fontsize=font_size) 200 | plt.plot(gen, max_dist, color='blue', linewidth=2.0, label='Max Distance') 201 | plt.legend(prop={'size': font_size}) 202 | plot_file_path = os.path.basename(datafile_path).replace('.csv', '') + '.png' 203 | plt.axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 204 | plt.savefig(os.path.join(plot_dir, plot_file_path)) 205 | plt.gcf().clear() 206 | 207 | return plot_file_path 208 | 209 | 210 | # Create a file to store the meaning of the different plots 211 | plot_key_file_path = os.path.join(plot_dir, 'ga_plots_key.csv') 212 | plot_key_file = open(plot_key_file_path, 'w') 213 | plot_key_file.write('Machine, Type,Run,Fitnes_Plot,Fitness_Data,Distance_Plot,Distance_Data\n') 214 | 215 | for ga_log in ga_logs: 216 | 217 | logfile_name = ga_log['log'] 218 | machine = ga_log['machine'] 219 | feedback = ga_log['feedback'] 220 | run = ga_log['run'] 221 | 222 | # Generate the data files 223 | fitness_data_file_path, max_dist_data_file_path = generate_fitness_datafile(os.path.join(log_dir, logfile_name)) 224 | 225 | # Generate the fitness plot 226 | plot_fitness_file_path = generate_fitness_plot(fitness_data_file_path, machine, feedback, run) 227 | 228 | # Generate the max distance plot 229 | plot_max_dist_file_path = generate_max_dist_plot(max_dist_data_file_path, machine, feedback, run) 230 | 231 | plot_key_file.write('{0},{1},{2},{3},{4},{5},{6}\n'.format(machine, 232 | feedback, 233 | run, 234 | os.path.basename(plot_fitness_file_path), 235 | os.path.basename(fitness_data_file_path), 236 | os.path.basename(plot_max_dist_file_path), 237 | os.path.basename(max_dist_data_file_path))) 238 | 239 | print 'Plot key file: {}'.format(plot_key_file_path) 240 | -------------------------------------------------------------------------------- /plot_scripts/keras_nw_graph.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | #from keras.initializers import normal, identity 3 | #from keras.models import model_from_json 4 | from keras.models import Sequential, Model 5 | from keras.layers import Dense, Flatten, Input, merge, Lambda 6 | #from keras.optimizers import Adam 7 | from keras import regularizers 8 | #import tensorflow as tf 9 | #import keras.backend as K 10 | from keras.initializers import RandomUniform 11 | from keras.utils import plot_model 12 | import os 13 | 14 | # Set the home directory 15 | home_dir = os.path.expanduser('~') 16 | 17 | # Set the directory for saving plots 18 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 19 | 20 | state_size=12 21 | action_dim=2 22 | HIDDEN1_UNITS = 400 23 | HIDDEN2_UNITS = 300 24 | 25 | # Drawing the critic network 26 | S = Input(shape=[state_size], name='states') 27 | A = Input(shape=[action_dim], name='action2') 28 | w1 = Dense( 29 | HIDDEN1_UNITS, activation='relu', 30 | kernel_regularizer=regularizers.l2(0.01), 31 | kernel_initializer=RandomUniform(minval=-1.0 / np.sqrt(state_size), maxval=1.0 / np.sqrt(state_size)), 32 | bias_initializer=RandomUniform(minval=-1.0 / np.sqrt(state_size), maxval=1.0 / np.sqrt(state_size)), 33 | name='w1', 34 | )(S) 35 | a1 = Dense( 36 | HIDDEN2_UNITS, activation='relu', 37 | kernel_regularizer=regularizers.l2(0.01), 38 | kernel_initializer=RandomUniform(minval=-1.0 / np.sqrt(action_dim), maxval=1.0 / np.sqrt(action_dim)), 39 | bias_initializer=RandomUniform(minval=-1.0 / np.sqrt(action_dim), maxval=1.0 / np.sqrt(action_dim)), 40 | name='a1', 41 | )(A) 42 | h1 = Dense( 43 | HIDDEN2_UNITS, activation='relu', 44 | kernel_regularizer=regularizers.l2(0.01), 45 | kernel_initializer=RandomUniform(minval=-1.0 / np.sqrt(HIDDEN1_UNITS), maxval=1.0 / np.sqrt(HIDDEN1_UNITS)), 46 | bias_initializer=RandomUniform(minval=-1.0 / np.sqrt(HIDDEN1_UNITS), maxval=1.0 / np.sqrt(HIDDEN1_UNITS)), 47 | name='h1', 48 | )(w1) 49 | h2 = merge([h1, a1], mode='sum', name='h2') 50 | h3 = Dense( 51 | HIDDEN2_UNITS, activation='relu', 52 | kernel_regularizer=regularizers.l2(0.01), 53 | kernel_initializer=RandomUniform(minval=-1.0 / np.sqrt(HIDDEN1_UNITS), maxval=1.0 / np.sqrt(HIDDEN1_UNITS)), 54 | bias_initializer=RandomUniform(minval=-1.0 / np.sqrt(HIDDEN1_UNITS), maxval=1.0 / np.sqrt(HIDDEN1_UNITS)), 55 | name='h3', 56 | )(h2) 57 | V = Dense( 58 | action_dim, activation='linear', # Not clear what this activation function should be 59 | kernel_initializer=RandomUniform(minval=-0.003, maxval=0.003), 60 | bias_initializer=RandomUniform(minval=-0.003, maxval=0.003), 61 | name='V')(h3) 62 | model = Model(input=[S, A], output=V) 63 | 64 | plot_model(model, to_file=os.path.join(plot_dir,'critic.png'), show_shapes=True) 65 | 66 | # Drawing the actor model 67 | S = Input(shape=[state_size]) 68 | # Use default initializer to initialize weights 69 | h0 = Dense(HIDDEN1_UNITS, activation='relu', 70 | kernel_initializer=RandomUniform(minval=-1.0/np.sqrt(state_size), maxval=1.0/np.sqrt(state_size)), 71 | bias_initializer=RandomUniform(minval=-1.0/np.sqrt(state_size), maxval=1.0/np.sqrt(state_size)) 72 | )(S) 73 | h1 = Dense(HIDDEN2_UNITS, activation='relu', 74 | kernel_initializer=RandomUniform(minval=-1.0 / np.sqrt(HIDDEN1_UNITS), maxval=1.0 / np.sqrt(HIDDEN1_UNITS)), 75 | bias_initializer=RandomUniform(minval=-1.0 / np.sqrt(HIDDEN1_UNITS), maxval=1.0 / np.sqrt(HIDDEN1_UNITS)) 76 | )(h0) 77 | Left_gain_factor = Dense(1,activation='sigmoid', # to bound output between 0 and 1 78 | kernel_initializer=RandomUniform(minval=-0.003, maxval=0.003), 79 | bias_initializer=RandomUniform(minval=-0.003, maxval=0.003))(h1) 80 | Right_gain_factor = Dense(1,activation='sigmoid', # to bound output between 0 and 1 81 | kernel_initializer=RandomUniform(minval=-0.003, maxval=0.003), 82 | bias_initializer=RandomUniform(minval=-0.003, maxval=0.003))(h1) 83 | V = merge([Left_gain_factor,Right_gain_factor],mode='concat') 84 | model = Model(input=S,output=V) 85 | 86 | plot_model(model, to_file=os.path.join(plot_dir,'actor.eps'), show_shapes=True) 87 | -------------------------------------------------------------------------------- /plot_scripts/rl_deviation_distance_tests_boxplot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import pandas as pd 5 | import os 6 | 7 | # Set the home directory 8 | home_dir = os.path.expanduser('~') 9 | 10 | # Set the directory for saving plots 11 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 12 | 13 | # Directory for saving plot data files 14 | plot_data_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plot_data') 15 | 16 | # Set the directory where logs are stored 17 | log_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/logs/reinforcement_learning_logs') 18 | 19 | # Function to create a dataframe from a reinforcement learning test log file 20 | def create_df(log_file_path): 21 | 22 | file_list = list() 23 | 24 | # First search for total reward at the end of test episodes 25 | # The line with thesearch string will contain the reward for the test episode 26 | # 2 lines above the search string will be the deviation and distance for the test episode 27 | # Equivalent grep command: grep -B2 '\[DDPG TEST\] TOTAL REWARD' log_20171029_203316.txt 28 | search_string = '[DDPG TEST] TOTAL REWARD' 29 | 30 | # Column names 31 | cols = ['episode', 'reward', 'deviation', 'distance', 'torso_orientation'] 32 | 33 | file_list = list() 34 | 35 | lines = open(log_file_path, "r").read().splitlines() 36 | 37 | curr_minus_line = '' 38 | curr_line = '' 39 | j = 2 40 | 41 | for i, line in enumerate(lines): 42 | if search_string in line: 43 | curr_line = line 44 | # Search for the line containing 'Deviation' above this line 45 | for k in range(5): 46 | if 'Deviation' in lines[i - k]: 47 | j=k 48 | curr_minus_line = lines[i - j] 49 | deviation = curr_minus_line.split(' ')[4] 50 | distance = curr_minus_line.split(' ')[6] 51 | torso_orientation = curr_minus_line.split(' ')[8] 52 | reward = curr_line.split(' ')[12] 53 | episode = curr_line.split(' ')[7].replace('-th', '') 54 | 55 | # Apend the current observations to the 1D list 56 | file_list.append([episode, reward, deviation, distance, torso_orientation]) 57 | 58 | # Crate a data frame by using the 1D list and reshaping it 59 | df = pd.DataFrame(np.array(file_list).reshape(100, 5), columns=cols) 60 | 61 | # Derive the name of the data file from the log file 62 | data_file_path = os.path.join(plot_data_dir, 'rl_test_data_' + os.path.basename(log_file_path)) 63 | 64 | print 'Saving {}'.format(data_file_path) 65 | 66 | return df 67 | 68 | # Function to create a dataframe from a static deviation test log file 69 | def create_df_static(log_file_path): 70 | file_list = list() 71 | 72 | # Lines with the search string will contain the distance, deviation and torso-gamma 73 | search_string_straight = 'STATIC TEST straight' 74 | 75 | # Column names 76 | cols = ['episode', 'deviation', 'distance', 'torso_orientation'] 77 | 78 | file_list_straight = list() 79 | 80 | lines = open(log_file_path, "r").read().splitlines() 81 | 82 | for i, line in enumerate(lines): 83 | 84 | if search_string_straight in line: 85 | episode = line.split(' ')[6] 86 | distance = line.split(' ')[8] 87 | deviation = line.split(' ')[10] 88 | torso_orientation = line.split(' ')[12] 89 | file_list_straight.append([episode, deviation, distance, torso_orientation]) 90 | 91 | # Crate a data frame by using the 1D list and reshaping it 92 | df_straight = pd.DataFrame(np.array(file_list_straight).reshape(100, 4), columns=cols) 93 | 94 | # Return the 3 dataframes 95 | return df_straight 96 | 97 | global figcount 98 | figcount = 1 99 | 100 | def save_boxplot(datafile, plotfile, type, time, limits): 101 | font_size = 36 102 | plt.rcParams.update({'font.size': font_size}) 103 | 104 | global figcount 105 | linewidth = 1 106 | # Create box plots using the data files saved above 107 | i, setup_control, setup1, setup2, setup3, setup4 = np.loadtxt(os.path.join(plot_data_dir, datafile), delimiter=',', unpack=True, skiprows=1) 108 | 109 | fig = plt.figure(figcount, figsize=(18,10)) 110 | figcount += 1 111 | ax = fig.add_subplot(111) 112 | setup_list = [setup_control, setup1, setup2, setup3, setup4] 113 | setup_list.reverse() 114 | bp = ax.boxplot(setup_list, 115 | showmeans=False, patch_artist=True, vert=False, 116 | meanprops=dict(marker='D', markeredgecolor='black', markerfacecolor='yellow'), 117 | flierprops=dict(marker='o', markerfacecolor='#888888', markersize=5, linestyle='none', markeredgecolor='black')) 118 | box_count = 0 119 | for box in bp['boxes']: 120 | # change outline color 121 | box.set(color='#000000', linewidth=linewidth) 122 | # change fill color 123 | if box_count == 4: 124 | box.set_facecolor('#FA8C8C') 125 | else: 126 | box.set_facecolor('#9D98FA') 127 | box_count += 1 128 | 129 | ## change color and linewidth of the whiskers 130 | for whisker in bp['whiskers']: 131 | whisker.set(color='#000000', linewidth=linewidth) 132 | 133 | ## change color and linewidth of the caps 134 | for cap in bp['caps']: 135 | cap.set(color='#000000', linewidth=linewidth) 136 | 137 | ## change color and linewidth of the medians 138 | for median in bp['medians']: 139 | median.set(color='#000000', linewidth=linewidth) 140 | 141 | for mean in bp['means']: 142 | mean.set(color='#000000', linewidth=linewidth) 143 | #mean.set_facecolor('#FF0000') 144 | 145 | ## change the style of fliers and their fill 146 | for flier in bp['fliers']: 147 | flier.set(marker='o', color='#E0636F', alpha=1.0, linewidth=linewidth) 148 | 149 | ax.set_xlim(limits) 150 | ax.set_xlabel('{0} after {1}s ({2})'.format(type, time, 'm' if not type=='Torso Orientation' else 'radians'), fontsize=font_size) 151 | #ax.set_ylabel('Different setups', fontsize=24) 152 | ylabel_list = [r'$Setup_{control}$', r'$Setup_1$', r'$Setup_2$', r'$Setup_3$', r'$Setup_4$'] 153 | ylabel_list.reverse() 154 | ax.set_yticklabels(ylabel_list, fontsize=font_size) 155 | # Color the Y labels according to the box colors 156 | colors = ['b', 'b', 'b', 'b', 'r'] 157 | for ytick, color in zip(ax.get_yticklabels(), colors): 158 | ytick.set_color(color) 159 | #ax.set_title('{0} for 100 test episodes of {1}s'.format(type, time)) 160 | ax.axvline(x=0.0, color='black', linestyle='-', linewidth=0.5) 161 | ax.grid(color='#222222', linestyle='dotted', linewidth=1) 162 | plt.savefig(os.path.join(plot_dir, plotfile), bbox_inches='tight') 163 | 164 | 165 | # Here we have 4 test logs for the 4 settings combinations and 2 more logs for the static tests 166 | 167 | # Setup: 1 168 | # Reward function: 1.0*(-abs(deviation)) + 0.5*forward_x + 1.0*(-abs(torso_gamma)) 169 | # Lowest possible gain: 0.1 170 | # Walk time: 40s 171 | # Machine: kogspc16 172 | test_log_setup1 = 'log_20171103_183052.txt' 173 | 174 | # Setup: 2 175 | # Reward function: 1.0*(-abs(deviation)) + 0.5*forward_x + 1.0*(-abs(torso_gamma)) 176 | # Lowest possible gain: 0.4 177 | # Walk time: 40s 178 | # Machine: kogspc37 179 | test_log_setup2 = 'log_20171103_181730.txt' 180 | 181 | # Setup: 3 182 | # Reward function: 1.0*(-abs(deviation)) + 0.3*forward_x + 1.0*(-abs(torso_gamma)) 183 | # Lowest possible gain: 0.1 184 | # Walk time: 40s 185 | # Machine: kogspc16 186 | test_log_setup3 = 'log_20171104_211720.txt' 187 | 188 | # Setup: 4 189 | # Reward function: 1.0*(-abs(deviation)) + 0.3*forward_x + 1.0*(-abs(torso_gamma)) 190 | # Lowest possible gain: 0.4 191 | # Walk time: 40s 192 | # Machine: kogspc37 193 | test_log_setup4 = 'log_20171104_211750.txt' 194 | 195 | # Setup: Static test (straight) 196 | # Walk time: 40s 197 | # Machine: kogspc37 198 | test_log_static = 'log_20171103_230940.txt' 199 | 200 | # Create dataframes from the test log files 201 | df_test_log_setup1 = create_df(os.path.join(log_dir, test_log_setup1)) 202 | df_test_log_setup2 = create_df(os.path.join(log_dir, test_log_setup2)) 203 | df_test_log_setup3 = create_df(os.path.join(log_dir, test_log_setup3)) 204 | df_test_log_setup4 = create_df(os.path.join(log_dir, test_log_setup4)) 205 | 206 | # Create dataframe for the static test 207 | df_test_log_static = create_df_static(os.path.join(log_dir, test_log_static)) 208 | 209 | # Now we need dataframes for the following parameters 210 | # 1. Distance 211 | # 2. Deviation 212 | # 3. Torso orientation 213 | # Each of these dataframes will have columns for the static test and each of the 4 setups 214 | 215 | col_names = ['straight', 'Setup 1', 'Setup 2', 'Setup 3', 'Setup 4'] 216 | 217 | # dataframe for distance 218 | df_boxplot_distance = pd.concat([df_test_log_static['distance'], 219 | df_test_log_setup1['distance'], 220 | df_test_log_setup2['distance'], 221 | df_test_log_setup3['distance'], 222 | df_test_log_setup4['distance'], 223 | ], axis=1) 224 | df_boxplot_distance.columns = col_names 225 | 226 | # dataframe for deviation 227 | df_boxplot_deviation = pd.concat([df_test_log_static['deviation'], 228 | df_test_log_setup1['deviation'], 229 | df_test_log_setup2['deviation'], 230 | df_test_log_setup3['deviation'], 231 | df_test_log_setup4['deviation'], 232 | ], axis=1) 233 | df_boxplot_deviation.columns = col_names 234 | 235 | # dataframe for torso_orientation 236 | df_boxplot_torso_orientation = pd.concat([df_test_log_static['torso_orientation'], 237 | df_test_log_setup1['torso_orientation'], 238 | df_test_log_setup2['torso_orientation'], 239 | df_test_log_setup3['torso_orientation'], 240 | df_test_log_setup4['torso_orientation'], 241 | ], axis=1) 242 | df_boxplot_torso_orientation.columns = col_names 243 | 244 | # Save the boxplot dataframes 245 | boxplot_datafilenames = ['rl_test_data_boxplot_distance.csv', 246 | 'rl_test_data_boxplot_deviation.csv', 247 | 'rl_test_data_boxplot_torso_orientation.csv'] 248 | 249 | df_boxplot_distance.to_csv(os.path.join(plot_data_dir, boxplot_datafilenames[0])) 250 | df_boxplot_deviation.to_csv(os.path.join(plot_data_dir, boxplot_datafilenames[1])) 251 | df_boxplot_torso_orientation.to_csv(os.path.join(plot_data_dir, boxplot_datafilenames[2])) 252 | 253 | 254 | # Create the plots 255 | save_boxplot(boxplot_datafilenames[0], 'rl_test_data_boxplot_distance.eps', 'Distance', 40, (-1, 7)) 256 | save_boxplot(boxplot_datafilenames[1], 'rl_test_data_boxplot_deviation.eps', 'Deviation', 40, (-3, 5)) 257 | save_boxplot(boxplot_datafilenames[2], 'rl_test_data_boxplot_torso_orientation.eps', 'Torso Orientation', 40, (-1, 3)) 258 | 259 | # Print the median and standard deviation for each setup and parameters 260 | 261 | for col in col_names: 262 | # col represents the different setups 263 | # Calculate the IRQs for the different parameters 264 | dist_q75, dist_q25 = np.percentile(df_boxplot_distance[col].apply(float), [75, 25]) 265 | dist_iqr = dist_q75 - dist_q25 266 | 267 | dev_q75, dev_q25 = np.percentile(df_boxplot_deviation[col].apply(float), [75, 25]) 268 | dev_iqr = dev_q75 - dev_q25 269 | 270 | gamma_q75, gamma_q25 = np.percentile(df_boxplot_torso_orientation[col].apply(float), [75, 25]) 271 | gamma_iqr = gamma_q75 - gamma_q25 272 | 273 | # Print the medians and IQRs 274 | print '{} & {} & {} & {} & {} & {} & {}'.format(col, 275 | "{0:.3f}".format(df_boxplot_distance[col].apply(float).median()), 276 | "{0:.3f}".format(df_boxplot_deviation[col].apply(float).median()), 277 | "{0:.3f}".format(df_boxplot_torso_orientation[col].apply(float).median()), 278 | "{0:.3f}".format(dist_iqr), 279 | "{0:.3f}".format(dev_iqr), 280 | "{0:.3f}".format(gamma_iqr)) 281 | 282 | print '' 283 | 284 | 285 | -------------------------------------------------------------------------------- /plot_scripts/rl_deviation_distance_tests_boxplot_paper.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import pandas as pd 5 | import os 6 | import matplotlib 7 | 8 | # For avoiding type-3 fonts 9 | matplotlib.rcParams['pdf.fonttype'] = 42 10 | matplotlib.rcParams['ps.fonttype'] = 42 11 | 12 | # Set the home directory 13 | home_dir = os.path.expanduser('~') 14 | 15 | # Set the directory for saving plots 16 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 17 | 18 | # Directory for saving plot data files 19 | plot_data_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plot_data') 20 | 21 | # Set the directory where logs are stored 22 | log_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/logs/reinforcement_learning_logs') 23 | 24 | 'rl_test_data_boxplot_distance.csv', 'rl_test_data_boxplot_deviation.csv', 'rl_test_data_boxplot_torso_orientation.csv' 25 | 'rl_test_data_boxplot_paper.eps', [(-1, 7), (-3, 5), (-1, 3)] 26 | 27 | def save_boxplot(datafile_distance, datafile_deviation, datafile_orientation, plotfile, limits): 28 | font_size = 40 29 | plt.rcParams.update({'font.size': font_size}) 30 | 31 | linewidth = 1 32 | # Create box plots using the data files saved above 33 | i_dist, setup_control_dist, setup1_dist, setup2_dist, setup3_dist, setup4_dist = np.loadtxt(os.path.join(plot_data_dir, datafile_distance), delimiter=',', unpack=True, skiprows=1) 34 | i_dev, setup_control_dev, setup1_dev, setup2_dev, setup3_dev, setup4_dev = np.loadtxt(os.path.join(plot_data_dir, datafile_deviation), delimiter=',', unpack=True, skiprows=1) 35 | i_or, setup_control_or, setup1_or, setup2_or, setup3_or, setup4_or = np.loadtxt(os.path.join(plot_data_dir, datafile_orientation), delimiter=',', unpack=True, skiprows=1) 36 | 37 | fig, ax = plt.subplots(3, 1, figsize=(24,20), sharey=False) 38 | #ax = fig.add_subplot(111) 39 | setup_list_dist = [setup_control_dist, setup1_dist, setup2_dist, setup3_dist, setup4_dist] 40 | setup_list_dev = [setup_control_dev, setup1_dev, setup2_dev, setup3_dev, setup4_dev] 41 | setup_list_or = [setup_control_or, setup1_or, setup2_or, setup3_or, setup4_or] 42 | 43 | setup_list_dist.reverse() 44 | setup_list_dev.reverse() 45 | setup_list_or.reverse() 46 | 47 | bp_dist = ax[0].boxplot(setup_list_dist, 48 | showmeans=False, patch_artist=True, vert=False, 49 | meanprops=dict(marker='D', markeredgecolor='black', markerfacecolor='yellow'), 50 | flierprops=dict(marker='o', markerfacecolor='#888888', markersize=5, linestyle='none', markeredgecolor='black')) 51 | 52 | bp_dev = ax[1].boxplot(setup_list_dev, 53 | showmeans=False, patch_artist=True, vert=False, 54 | meanprops=dict(marker='D', markeredgecolor='black', markerfacecolor='yellow'), 55 | flierprops=dict(marker='o', markerfacecolor='#888888', markersize=5, linestyle='none', markeredgecolor='black')) 56 | 57 | bp_or = ax[2].boxplot(setup_list_or, 58 | showmeans=False, patch_artist=True, vert=False, 59 | meanprops=dict(marker='D', markeredgecolor='black', markerfacecolor='yellow'), 60 | flierprops=dict(marker='o', markerfacecolor='#888888', markersize=5, linestyle='none', markeredgecolor='black')) 61 | 62 | box_count = 0 63 | for boxes in zip(bp_dist['boxes'], bp_dev['boxes'], bp_or['boxes']): 64 | # change outline color 65 | for box in boxes: box.set(color='#000000', linewidth=linewidth) 66 | # change fill color 67 | if box_count == 4: 68 | for box in boxes: box.set_facecolor('#FA8C8C') 69 | else: 70 | for box in boxes: box.set_facecolor('#9D98FA') 71 | box_count += 1 72 | 73 | ## change color and linewidth of the whiskers 74 | for whiskers in zip(bp_dist['whiskers'], bp_dev['whiskers'], bp_or['whiskers']): 75 | for whisker in whiskers: whisker.set(color='#000000', linewidth=2.5*linewidth) 76 | 77 | ## change color and linewidth of the caps 78 | for caps in zip(bp_dist['caps'], bp_dev['caps'], bp_or['caps']): 79 | for cap in caps: cap.set(color='#000000', linewidth=linewidth) 80 | 81 | ## change color and linewidth of the medians 82 | for medians in zip(bp_dist['medians'], bp_dev['medians'], bp_or['medians']): 83 | for median in medians: median.set(color='#000000', linewidth=linewidth) 84 | 85 | #for mean in bp['means']: 86 | #mean.set(color='#000000', linewidth=linewidth) 87 | #mean.set_facecolor('#FF0000') 88 | 89 | ## change the style of fliers and their fill 90 | for fliers in zip(bp_dist['fliers'], bp_dev['fliers'], bp_or['fliers']): 91 | for flier in fliers: flier.set(marker='o', color='#E0636F', alpha=1.0, linewidth=linewidth) 92 | 93 | ax[0].set_xlim(limits[0]) 94 | ax[0].set_xlabel('Distance after 40s (m)') 95 | ax[0].tick_params(axis='y', pad=15) 96 | ax[0].tick_params(axis='x', pad=15) 97 | 98 | ax[1].set_xlim(limits[1]) 99 | ax[1].set_xlabel('Deviation after 40s (m)') 100 | ax[1].tick_params(axis='y', pad=15) 101 | ax[1].tick_params(axis='x', pad=15) 102 | 103 | ax[2].set_xlim(limits[2]) 104 | ax[2].set_xlabel('Orientation after 40s (radians)') 105 | ax[2].tick_params(axis='y', pad=15) 106 | ax[2].tick_params(axis='x', pad=15) 107 | 108 | ylabel_list = ['S0', 'S1', 'S2', 'S3', 'S4'] 109 | ylabel_list.reverse() 110 | for i in range(3): 111 | ax[i].set_yticklabels(ylabel_list, fontsize=font_size) 112 | # Color the Y labels according to the box colors 113 | colors = ['b', 'b', 'b', 'b', 'r'] 114 | for ytick, color in zip(ax[i].get_yticklabels(), colors): 115 | ytick.set_color(color) 116 | 117 | ax[i].axvline(x=0.0, color='black', linestyle='-', linewidth=0.5) 118 | ax[i].grid(color='#222222', linestyle='dotted', linewidth=1) 119 | 120 | #plt.show() 121 | plt.tight_layout() 122 | plt.savefig(os.path.join(plot_dir, plotfile), bbox_inches='tight') 123 | 124 | save_boxplot('rl_test_data_boxplot_distance.csv', 125 | 'rl_test_data_boxplot_deviation.csv', 126 | 'rl_test_data_boxplot_torso_orientation.csv', 127 | 'rl_test_data_boxplot_paper.eps', 128 | [(-1, 7), (-3, 5), (-1, 3)]) 129 | -------------------------------------------------------------------------------- /plot_scripts/rl_reward_deviation_distance_vs_episodes.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import pandas as pd 5 | import os 6 | 7 | # Set the home directory 8 | home_dir = os.path.expanduser('~') 9 | 10 | # Set the directory for saving plots 11 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 12 | 13 | # Directory for saving plot data files 14 | plot_data_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plot_data') 15 | 16 | # Set the directory where logs are stored 17 | log_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/logs/reinforcement_learning_logs') 18 | 19 | 20 | # For each training log file, 2 plots are required (reward vs test episode, distance and deviation vs test episode) 21 | # Function to create a dataframe from a reinforcement learning training log file 22 | def create_data_file(log_file_path): 23 | 24 | file_list = list() 25 | 26 | # First search for total reward at the end of test episodes 27 | # The line with thesearch string will contain the reward for the test episode 28 | # 2 lines above the search string will be the deviation and distance for the test episode 29 | # Equivalent grep command: grep -B2 '\[DDPG TEST\] TOTAL REWARD' log_20171029_203316.txt 30 | search_string = '[DDPG TEST] TOTAL REWARD' 31 | 32 | # Column names 33 | cols = ['episode', 'reward', 'deviation', 'distance', 'torso_gamma'] 34 | 35 | file_list = list() 36 | 37 | lines = open(log_file_path, "r").read().splitlines() 38 | 39 | curr_minus_line = '' 40 | curr_line = '' 41 | j = 2 42 | 43 | for i, line in enumerate(lines): 44 | if search_string in line: 45 | curr_line = line 46 | # Search for the line containing 'Deviation' above this line 47 | for k in range(5): 48 | if 'Deviation' in lines[i - k]: 49 | j=k 50 | curr_minus_line = lines[i - j] 51 | deviation = curr_minus_line.split(' ')[4] 52 | distance = curr_minus_line.split(' ')[6] 53 | torso_gamma = curr_minus_line.split(' ')[8] 54 | reward = curr_line.split(' ')[12] 55 | episode = curr_line.split(' ')[7].replace('-th', '') 56 | 57 | # Apend the current observations to the 1D list for first 500 episodes 58 | if int(episode) > 500 : pass 59 | file_list.append([episode, reward, deviation, distance, torso_gamma]) 60 | 61 | # Crate a data frame by using the 1D list and reshaping it 62 | df = pd.DataFrame(np.array(file_list).reshape(99, 5), columns=cols) 63 | 64 | # Derive the name of the data file from the log file 65 | data_file_path = os.path.join(plot_data_dir, 'rl_train_data_' + os.path.basename(log_file_path)) 66 | 67 | print 'Saving {}'.format(data_file_path) 68 | 69 | # Store the dataframe as a csv in the data folder 70 | df.to_csv(data_file_path, sep=',') 71 | 72 | return data_file_path 73 | 74 | 75 | # Dict of RL training logs and plot titles 76 | rl_log_plot_titles = {'log_20171102_191656.txt': 'Setup 1', 77 | 'log_20171102_195120.txt': 'Setup 2', 78 | 'log_20171104_010554.txt': 'Setup 3', 79 | 'log_20171104_010409.txt': 'Setup 4' 80 | } 81 | 82 | data_file_list = [] 83 | # Create a data file for each log file and append the file name to the list 84 | # This list will be used for generating plots 85 | for logfile in rl_log_plot_titles.keys(): 86 | data_file_list.append(create_data_file(os.path.join(log_dir,logfile))) 87 | 88 | # Change the font size used in the plots 89 | font_size = 12 90 | plt.rcParams.update({'font.size': font_size}) 91 | 92 | plotcount = 1 93 | for data_file in data_file_list: 94 | i, episodes, rewards, deviations, distances, torso_gamma = np.loadtxt(data_file, delimiter=',', unpack=True, skiprows=1) 95 | 96 | plt.figure(plotcount) 97 | plt.xlabel('Episode') 98 | plt.ylabel('Total reward for episode') 99 | plt.ylim(-150,60) 100 | reward_plot_title = 'Rewards vs episodes | ' + rl_log_plot_titles[os.path.basename(data_file).replace('rl_train_data_', '')] 101 | plt.title(reward_plot_title, fontsize=font_size) 102 | plt.plot(episodes, rewards, color='g', linewidth=2.0, label='Reward') 103 | plt.legend(prop={'size': font_size}) 104 | plot_file_name1 = os.path.basename(data_file).replace('.txt', '') + '_reward.png' 105 | plt.axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 106 | plt.savefig(os.path.join(plot_dir, plot_file_name1)) 107 | 108 | plt.figure(plotcount + 1) 109 | plt.xlabel('Episode') 110 | plt.ylabel('Deviation/Distance (m)') 111 | plt.ylim((-6,8)) 112 | dev_plot_title = 'Distance/deviation vs episodes | ' + rl_log_plot_titles[os.path.basename(data_file).replace('rl_train_data_', '')] 113 | plt.title(dev_plot_title, fontsize=font_size) 114 | plt.plot(episodes, deviations, color='r', linewidth=2.0, label='Deviation') 115 | plt.plot(episodes, distances, color='b', linewidth=2.0, label='Distance') 116 | plot_file_name2 = os.path.basename(data_file).replace('.txt', '') + '_deviation.png' 117 | plt.legend(prop={'size': font_size}) 118 | # Draw a horizontal line at 0 for reference 119 | plt.axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 120 | plt.savefig(os.path.join(plot_dir, plot_file_name2)) 121 | 122 | plt.figure(plotcount + 2) 123 | plt.xlabel('Episode') 124 | plt.ylabel('Torso orientation (radians)') 125 | plt.ylim((-3, 3)) 126 | dev_plot_title = 'Torso gamma angle vs episodes | ' + rl_log_plot_titles[os.path.basename(data_file).replace('rl_train_data_', '')] 127 | plt.title(dev_plot_title, fontsize=font_size) 128 | plt.plot(episodes, torso_gamma, color='brown', linewidth=2.0, label='Torso orientation') 129 | plot_file_name2 = os.path.basename(data_file).replace('.txt', '') + '_orientation.png' 130 | plt.legend(prop={'size': font_size}) 131 | # Draw a horizontal line at 0 for reference 132 | plt.axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 133 | plt.savefig(os.path.join(plot_dir, plot_file_name2)) 134 | 135 | plotcount += 3 136 | 137 | 138 | -------------------------------------------------------------------------------- /plot_scripts/rl_reward_deviation_distance_vs_episodes_subplots.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import pandas as pd 5 | import os 6 | 7 | # Set the home directory 8 | home_dir = os.path.expanduser('~') 9 | 10 | # Set the directory for saving plots 11 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 12 | 13 | # Directory for saving plot data files 14 | plot_data_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plot_data') 15 | 16 | # Set the directory where logs are stored 17 | log_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/logs/reinforcement_learning_logs') 18 | 19 | 20 | # For each training log file, 2 plots are required (reward vs test episode, distance and deviation vs test episode) 21 | # Function to create a dataframe from a reinforcement learning training log file 22 | def create_data_file(log_file_path): 23 | 24 | file_list = list() 25 | 26 | # First search for total reward at the end of test episodes 27 | # The line with thesearch string will contain the reward for the test episode 28 | # 2 lines above the search string will be the deviation and distance for the test episode 29 | # Equivalent grep command: grep -B2 '\[DDPG TEST\] TOTAL REWARD' log_20171029_203316.txt 30 | search_string = '[DDPG TEST] TOTAL REWARD' 31 | 32 | # Column names 33 | cols = ['episode', 'reward', 'deviation', 'distance', 'torso_gamma'] 34 | 35 | file_list = list() 36 | 37 | lines = open(log_file_path, "r").read().splitlines() 38 | 39 | curr_minus_line = '' 40 | curr_line = '' 41 | j = 2 42 | 43 | for i, line in enumerate(lines): 44 | if search_string in line: 45 | curr_line = line 46 | # Search for the line containing 'Deviation' above this line 47 | for k in range(5): 48 | if 'Deviation' in lines[i - k]: 49 | j=k 50 | curr_minus_line = lines[i - j] 51 | deviation = curr_minus_line.split(' ')[4] 52 | distance = curr_minus_line.split(' ')[6] 53 | torso_gamma = curr_minus_line.split(' ')[8] 54 | reward = curr_line.split(' ')[12] 55 | episode = curr_line.split(' ')[7].replace('-th', '') 56 | 57 | # Apend the current observations to the 1D list for first 500 episodes 58 | if int(episode) > 500 : pass 59 | file_list.append([episode, reward, deviation, distance, torso_gamma]) 60 | 61 | # Crate a data frame by using the 1D list and reshaping it 62 | df = pd.DataFrame(np.array(file_list).reshape(99, 5), columns=cols) 63 | 64 | # Derive the name of the data file from the log file 65 | data_file_path = os.path.join(plot_data_dir, 'rl_train_data_' + os.path.basename(log_file_path)) 66 | 67 | print 'Saving {}'.format(data_file_path) 68 | 69 | # Store the dataframe as a csv in the data folder 70 | df.to_csv(data_file_path, sep=',') 71 | 72 | return data_file_path 73 | 74 | 75 | # Dict of RL training logs and plot titles 76 | rl_log_plot_titles = {'log_20171102_191656.txt': 'Setup1', 77 | 'log_20171102_195120.txt': 'Setup2', 78 | 'log_20171104_010554.txt': 'Setup3', 79 | 'log_20171104_010409.txt': 'Setup4' 80 | } 81 | 82 | data_file_list = [] 83 | # Create a data file for each log file and append the file name to the list 84 | # This list will be used for generating plots 85 | for logfile in rl_log_plot_titles.keys(): 86 | data_file_list.append(create_data_file(os.path.join(log_dir,logfile))) 87 | 88 | # Change the font size used in the plots 89 | font_size = 20 90 | plt.rcParams.update({'font.size': font_size}) 91 | 92 | plotcount = 1 93 | for data_file in data_file_list: 94 | 95 | plot_file_name_sub = '' 96 | for k,v in rl_log_plot_titles.items(): 97 | if k in data_file: 98 | plot_file_name_sub = v 99 | 100 | i, episodes, rewards, deviations, distances, torso_gamma = np.loadtxt(data_file, delimiter=',', unpack=True, skiprows=1) 101 | 102 | fig, ax = plt.subplots(3, figsize=(18, 10), sharex=True, sharey=False) 103 | 104 | # Set the line width 105 | linewidth = 3.5 106 | 107 | # Whether to show grids or not 108 | grid = True 109 | 110 | # Set the x-axis limits 111 | ax[0].set_xlim([0, 1000]) 112 | ax[1].set_xlim([0, 1000]) 113 | ax[2].set_xlim([0, 1000]) 114 | 115 | # Set the y-axis limits 116 | ax[0].set_ylim([-145,75]) 117 | ax[1].set_ylim([-6,7.9]) 118 | ax[2].set_ylim([-2.9, 2.9]) 119 | 120 | # Set the axis labels (since x-axis is shared, only the last subplot has an xlabel) 121 | ax[2].set_xlabel('Episode', fontsize=22) 122 | 123 | ax[0].set_ylabel('Reward', fontsize=22, horizontalalignment='center') 124 | ax[0].yaxis.set_ticks_position('both') 125 | ax[1].set_ylabel('Deviation/\nDistance (m)', fontsize=22, horizontalalignment='center') 126 | ax[1].yaxis.set_ticks_position('both') 127 | ax[2].set_ylabel('Torso orientation \n(radians)', fontsize=22, horizontalalignment='center') 128 | ax[2].yaxis.set_ticks_position('both') 129 | 130 | # First subplot contains max and avg fitness for run 1 131 | l0, = ax[0].plot(episodes, rewards, color='green', linewidth=linewidth, label='Reward') 132 | ax[0].axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 133 | if grid: ax[0].grid(color='#222222', linestyle='dotted', linewidth=1) 134 | 135 | # Second subplot contains max and avg fitness for run 2 136 | l11, = ax[1].plot(episodes, deviations, color='red', linewidth=linewidth, label='Deviation') 137 | l12, = ax[1].plot(episodes, distances, color='blue', linewidth=linewidth, label='Distance') 138 | ax[1].axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 139 | if grid: ax[1].grid(color='#222222', linestyle='dotted', linewidth=1) 140 | 141 | # Third subplot contains max and avg fitness for run 3 142 | l2, = ax[2].plot(episodes, torso_gamma, color='#734607', linewidth=linewidth, label='Torso orientation') 143 | ax[2].axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 144 | if grid: ax[2].grid(color='#222222', linestyle='dotted', linewidth=1) 145 | 146 | plt.subplots_adjust(hspace=0.1) 147 | plt.legend(handles=[l0, l11, l12, l2], bbox_to_anchor=(1.01, 3.55), loc='upper right', ncol=4, fontsize=20) 148 | 149 | plot_file_name = 'rl_train_' + plot_file_name_sub + '.svg' 150 | plt.savefig(os.path.join(plot_dir, plot_file_name), bbox_inches='tight') 151 | -------------------------------------------------------------------------------- /plot_scripts/rl_reward_deviation_distance_vs_episodes_subplots_paper.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import rcParams 3 | import matplotlib.pyplot as plt 4 | import pandas as pd 5 | import os 6 | import matplotlib 7 | 8 | # For avoiding type-3 fonts 9 | matplotlib.rcParams['pdf.fonttype'] = 42 10 | matplotlib.rcParams['ps.fonttype'] = 42 11 | 12 | # Set the home directoryfrom matplotlib import rcParams 13 | 14 | home_dir = os.path.expanduser('~') 15 | 16 | # Set the directory for saving plots 17 | plot_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plots') 18 | 19 | # Directory for saving plot data files 20 | plot_data_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/plot_data') 21 | 22 | # Set the directory where logs are stored 23 | log_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/logs/reinforcement_learning_logs') 24 | 25 | 26 | # For each training log file, 2 plots are required (reward vs test episode, distance and deviation vs test episode) 27 | # Function to create a dataframe from a reinforcement learning training log file 28 | def create_data_file(log_file_path): 29 | 30 | file_list = list() 31 | 32 | # First search for total reward at the end of test episodes 33 | # The line with thesearch string will contain the reward for the test episode 34 | # 2 lines above the search string will be the deviation and distance for the test episode 35 | # Equivalent grep command: grep -B2 '\[DDPG TEST\] TOTAL REWARD' log_20171029_203316.txt 36 | search_string = '[DDPG TEST] TOTAL REWARD' 37 | 38 | # Column names 39 | cols = ['episode', 'reward', 'deviation', 'distance', 'torso_gamma'] 40 | 41 | file_list = list() 42 | 43 | lines = open(log_file_path, "r").read().splitlines() 44 | 45 | curr_minus_line = '' 46 | curr_line = '' 47 | j = 2 48 | 49 | for i, line in enumerate(lines): 50 | if search_string in line: 51 | curr_line = line 52 | # Search for the line containing 'Deviation' above this line 53 | for k in range(5): 54 | if 'Deviation' in lines[i - k]: 55 | j=k 56 | curr_minus_line = lines[i - j] 57 | deviation = curr_minus_line.split(' ')[4] 58 | distance = curr_minus_line.split(' ')[6] 59 | torso_gamma = curr_minus_line.split(' ')[8] 60 | reward = curr_line.split(' ')[12] 61 | episode = curr_line.split(' ')[7].replace('-th', '') 62 | 63 | # Apend the current observations to the 1D list for first 500 episodes 64 | if int(episode) > 500 : pass 65 | file_list.append([episode, reward, deviation, distance, torso_gamma]) 66 | 67 | # Crate a data frame by using the 1D list and reshaping it 68 | df = pd.DataFrame(np.array(file_list).reshape(99, 5), columns=cols) 69 | 70 | # Derive the name of the data file from the log file 71 | data_file_path = os.path.join(plot_data_dir, 'rl_train_data_' + os.path.basename(log_file_path)) 72 | 73 | print 'Saving {}'.format(data_file_path) 74 | 75 | # Store the dataframe as a csv in the data folder 76 | df.to_csv(data_file_path, sep=',') 77 | 78 | return data_file_path 79 | 80 | 81 | # Dict of RL training logs and plot titles 82 | rl_log_plot_titles = {'log_20171102_191656.txt': 'Setup1', 83 | 'log_20171102_195120.txt': 'Setup2', 84 | 'log_20171104_010554.txt': 'Setup3', 85 | 'log_20171104_010409.txt': 'Setup4' 86 | } 87 | 88 | data_file_list = [] 89 | # Create a data file for each log file and append the file name to the list 90 | # This list will be used for generating plots 91 | for logfile in rl_log_plot_titles.keys(): 92 | data_file_list.append(create_data_file(os.path.join(log_dir,logfile))) 93 | 94 | # Change the font size used in the plots 95 | font_size = 32 96 | plt.rcParams.update({'font.size': font_size}) 97 | 98 | # Each column for one data file 99 | fig, ax = plt.subplots(3, 4, figsize=(42, 10), sharex=True, sharey=False) 100 | 101 | # Set the line width 102 | linewidth = 3.5 103 | # Whether to show grids or not 104 | grid = True 105 | 106 | filecount = 0 107 | for data_file in data_file_list: 108 | 109 | i, episodes, rewards, deviations, distances, torso_gamma = np.loadtxt(data_file, delimiter=',', unpack=True, skiprows=1) 110 | 111 | # Set the x-axis limits 112 | ax[0][filecount].set_xlim([0, 1000]) 113 | ax[1][filecount].set_xlim([0, 1000]) 114 | ax[2][filecount].set_xlim([0, 1000]) 115 | 116 | # Set the y-axis limits 117 | ax[0][filecount].set_ylim([-145,75]) 118 | ax[1][filecount].set_ylim([-6,7.9]) 119 | ax[2][filecount].set_ylim([-2.9, 2.9]) 120 | 121 | # Set the plot titles 122 | ax[0][0].set_title('Setup S1', y=1.05, fontsize=32) 123 | ax[0][1].set_title('Setup S2', y=1.05, fontsize=32) 124 | ax[0][2].set_title('Setup S3', y=1.05, fontsize=32) 125 | ax[0][3].set_title('Setup S4', y=1.05, fontsize=32) 126 | 127 | 128 | # Set the axis labels (since x-axis is shared, only the last subplot has an xlabel) 129 | ax[2][filecount].set_xlabel('Episode', fontsize=font_size) 130 | plt.setp(ax[2][filecount].get_xticklabels(), rotation=30, fontsize=font_size, ha='right', rotation_mode='anchor') 131 | 132 | # Set y labels only for the first column 133 | if filecount == 0: 134 | ax[0][filecount].set_ylabel('Reward', fontsize=font_size, horizontalalignment='center') 135 | ax[0][filecount].get_yaxis().set_label_coords(-0.21,0.5) 136 | ax[0][filecount].tick_params(axis='y', pad=15) 137 | #ax[0][filecount].set_yticklabels([-150, -100, -50, 0, 50, visible=True, fontsize=30) 138 | else: 139 | ax[0][filecount].set_yticklabels([], visible=False) 140 | 141 | ax[0][filecount].yaxis.set_ticks_position('both') 142 | 143 | if filecount == 0: 144 | ax[1][filecount].set_ylabel('Deviation/\nDistance (m)', fontsize=font_size, horizontalalignment='center') 145 | ax[1][filecount].get_yaxis().set_label_coords(-0.15,0.5) 146 | ax[1][filecount].set_yticklabels(['',-4,'',0,'',4,''], visible=True) 147 | ax[1][filecount].tick_params(axis='y', pad=15) 148 | else: 149 | if filecount == 3: 150 | #ax[1][filecount].yaxis.tick_right() 151 | #ax[1][filecount].yaxis.set_ticks_position('both') 152 | ax[1][filecount].set_yticklabels([], visible=False) 153 | else: 154 | ax[1][filecount].set_yticklabels([], visible=False) 155 | ax[1][filecount].yaxis.set_ticks_position('both') 156 | 157 | if filecount == 0: 158 | ax[2][filecount].set_ylabel('Orientation \n(radians)', fontsize=font_size, horizontalalignment='center') 159 | ax[2][filecount].get_yaxis().set_label_coords(-0.15,0.5) 160 | ax[2][filecount].tick_params(axis='y', pad=15) 161 | else: 162 | if filecount == 3: 163 | #ax[2][filecount].yaxis.tick_right() 164 | #ax[2][filecount].yaxis.set_ticks_position('both') 165 | ax[2][filecount].set_yticklabels([], visible=False) 166 | else: 167 | ax[2][filecount].set_yticklabels([], visible=False) 168 | ax[2][filecount].yaxis.set_ticks_position('both') 169 | 170 | # First subplot contains max and avg fitness for run 1 171 | l0, = ax[0][filecount].plot(episodes, rewards, color='green', linewidth=linewidth, label='Reward') 172 | ax[0][filecount].axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 173 | if grid: ax[0][filecount].grid(color='#222222', linestyle='dotted', linewidth=1) 174 | 175 | # Second subplot contains max and avg fitness for run 2 176 | l11, = ax[1][filecount].plot(episodes, deviations, color='red', linewidth=linewidth, label='Deviation') 177 | l12, = ax[1][filecount].plot(episodes, distances, color='blue', linewidth=linewidth, label='Distance') 178 | ax[1][filecount].axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 179 | if grid: ax[1][filecount].grid(color='#222222', linestyle='dotted', linewidth=1) 180 | 181 | # Third subplot contains max and avg fitness for run 3 182 | l2, = ax[2][filecount].plot(episodes, torso_gamma, color='#734607', linewidth=linewidth, label='Orientation') 183 | ax[2][filecount].axhline(y=0.0, color='black', linestyle='-', linewidth=0.5) 184 | if grid: ax[2][filecount].grid(color='#222222', linestyle='dotted', linewidth=1) 185 | 186 | filecount += 1 187 | 188 | #plt.setp([a.get_xticklabels() for a in ax[0, :]], visible=False) 189 | #plt.setp([a.get_yticklabels() for a in x[:, 1:1:3]], visible=False) 190 | 191 | plt.subplots_adjust(hspace=0.1) 192 | plt.subplots_adjust(wspace=0.07) 193 | plt.legend(handles=[l0, l11, l12, l2], bbox_to_anchor=(1.036, 3.9), loc='upper right', ncol=4, fontsize=33, borderpad=0.1) 194 | 195 | plot_file_name = 'rl_train_paper.eps' 196 | plt.savefig(os.path.join(plot_dir, plot_file_name), bbox_inches='tight') 197 | -------------------------------------------------------------------------------- /shell_scripts/asus_20171005.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_2.py 3 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_3.py 4 | echo 'Done!' 5 | -------------------------------------------------------------------------------- /shell_scripts/find_acronyms.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | import shlex 3 | import re 4 | import os 5 | 6 | home_dir = os.path.expanduser('~') 7 | 8 | # Set the directory for saving plots 9 | report_dir = os.path.join(home_dir, 'computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/report') 10 | 11 | FOLDER = report_dir 12 | THESIS = os.path.join(report_dir,'thesis.pdf') 13 | OUTPUT_FILE = os.path.join(report_dir,'acronymsInMyThesis.txt') 14 | PATTERN = '[A-Z][A-Z]+' 15 | 16 | def searchAcronymsInPDF(): 17 | output = pdfSearch() 18 | acrs = [] 19 | for reg in re.findall(PATTERN, output): 20 | reg.strip() 21 | if (len(reg)>1): 22 | acrs.append(reg) 23 | return set(acrs) 24 | 25 | def pdfSearch(): 26 | command = 'pdfgrep "%s" %s'%(PATTERN,THESIS) 27 | output = shellCall(command) 28 | return output 29 | 30 | def shellCall(command): 31 | p = subprocess.Popen(shlex.split(command), stdout=subprocess.PIPE) 32 | out, _ = p.communicate() 33 | return out 34 | 35 | if __name__ == '__main__': 36 | acrs = searchAcronymsInPDF() 37 | acrs_list = list(acrs) 38 | print(sorted(acrs_list)) -------------------------------------------------------------------------------- /shell_scripts/logreader.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | logfile=$1 4 | echo "Logfile: $logfile" 5 | 6 | # Create a file to store the results 7 | resultfile=$(echo $logfile| sed 's/\.[^.]*$//')_results.txt 8 | touch $resultfile 9 | 10 | echo "Resultfile: $resultfile" 11 | echo "Logfile: $logfile" >> $resultfile 12 | echo "Resultfile: $resultfile" >> $resultfile 13 | echo "" >> $resultfile 14 | 15 | # Read the Min fitness for each generation 16 | echo 'Min fitness for generations:' >> $resultfile 17 | grep 'Min' $logfile | cut -d' ' -f5 >> $resultfile 18 | echo "" >> $resultfile 19 | 20 | # Read the Max fitness for each generation 21 | echo 'Max fitness for generations:' >> $resultfile 22 | grep 'Max' $logfile | cut -d' ' -f5 >> $resultfile 23 | echo "" >> $resultfile 24 | 25 | # Read the Avg fitness for each generation 26 | echo 'Avg fitness for generations:' >> $resultfile 27 | grep 'Avg' $logfile | cut -d' ' -f5 >> $resultfile 28 | echo "" >> $resultfile 29 | 30 | # Read the Std fitness for each generation 31 | echo 'Std fitness for generations:' >> $resultfile 32 | grep 'Std' $logfile | cut -d' ' -f5 >> $resultfile 33 | echo "" >> $resultfile 34 | 35 | # Read the x-distance and up times for the Max fitnesses 36 | listVar=$(grep 'Max' $logfile | cut -d' ' -f5) 37 | 38 | # Read the x-distances 39 | echo "X-Distances for max fitnesses" >> $resultfile 40 | for maxfit in $listVar; do 41 | maxdist=$(grep -B15 "fitness: $maxfit" $logfile | grep 'end_x' | cut -d' ' -f4 | sed 's/end_x=//' | sed 's/,//') 42 | echo $maxdist >> $resultfile 43 | done 44 | echo "" >> $resultfile 45 | 46 | # Read the up_times 47 | echo "Up times for max fitnesses" >> $resultfile 48 | for maxfit in $listVar; do 49 | up_time=$(grep -B15 "fitness: $maxfit" $logfile | grep 'up_time' | cut -d' ' -f4 | sed 's/up_time=//' | sed 's/fitness//') 50 | echo $up_time >> $resultfile 51 | done 52 | echo "" >> $resultfile 53 | -------------------------------------------------------------------------------- /shell_scripts/run_anglefeedback.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_3.py 3 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_3.py 4 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_3.py 5 | echo 'Done!' 6 | -------------------------------------------------------------------------------- /shell_scripts/run_openloop.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_2.py 3 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_2.py 4 | #python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_2.py 5 | echo 'Done!' 6 | -------------------------------------------------------------------------------- /shell_scripts/run_phasereset.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_5_1.py 3 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_5_1.py 4 | python $HOME/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/matsuoka_walk/ga_5_1.py 5 | echo 'Done!' 6 | -------------------------------------------------------------------------------- /shell_scripts/svg_to_eps.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | image_path="${HOME}/computing/repositories/MScThesis_SayantanAuddy_2017_NICOOscillatorWalking/report/src" 3 | filename_svg=${1} 4 | filename_eps=$(echo $filename_svg | cut -f 1 -d '.')'.eps' 5 | inkscape ${image_path}/${filename_svg} -E ${image_path}/${filename_eps} --export-ignore-filters 6 | echo "Converted ${filename_svg} to ${filename_eps}" -------------------------------------------------------------------------------- /vrep_scripts/remoteApiConnections.txt: -------------------------------------------------------------------------------- 1 | // This file defines all the continuous remote API server services (started at remote API plugin initialization, i.e. V-REP start-up) 2 | // 3 | // Each remote API server service requires following 3 entries: 4 | // 5 | // portIndex@_port = xxxx // where xxxx is the desired port number (below 19997 are preferred for server services starting at V-REP start-up) 6 | // portIndex@_debug = xxxx // where xxxx is true or false 7 | // portIndex@_syncSimTrigger = xxxx // where xxxx is true or false. When true, then the service will be pre-enabled for synchronous operation. 8 | // 9 | // In above strings, @ can be any number starting with 1. If more than one server service is required, then numbers need to be consecutive and starting with 1 10 | 11 | // Let's start a continuous remote API server service on port 19997: 12 | portIndex1_port = 19997 13 | portIndex1_debug = false 14 | portIndex1_syncSimTrigger = true 15 | 16 | // Let's start a continuous remote API server service on port 19998: 17 | portIndex2_port = 19998 18 | portIndex2_debug = false 19 | portIndex2_syncSimTrigger = true 20 | 21 | -------------------------------------------------------------------------------- /vrep_scripts/start_vrep.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Use the below command to make sure '.' is used as the decimal separator and not ',' 3 | 4 | LC_NUMERIC=en_US.UTF-8 ./vrep.sh 5 | 6 | -------------------------------------------------------------------------------- /vrep_scripts/start_vrep_headless.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | LC_NUMERIC=en_US.UTF-8 xvfb-run --auto-servernum --server-num=1 ./vrep.sh -h 4 | 5 | -------------------------------------------------------------------------------- /vrep_scripts/torso_11_respondable.lua: -------------------------------------------------------------------------------- 1 | function subscriber_callback(msg) 2 | -- This is the subscriber callback function 3 | simAddStatusbarMessage('subscriber receiver following Float32: '..msg.data) 4 | end 5 | 6 | function getTransformStamped(objHandle,name,relTo,relToName) 7 | -- This function retrieves the stamped transform for a specific object 8 | t=simGetSystemTime() 9 | p=simGetObjectPosition(objHandle,relTo) 10 | o=simGetObjectQuaternion(objHandle,relTo) 11 | return { 12 | header={ 13 | stamp=t, 14 | frame_id=relToName 15 | }, 16 | child_frame_id=name, 17 | transform={ 18 | translation={x=p[1],y=p[2],z=p[3]}, 19 | rotation={x=o[1],y=o[2],z=o[3],w=o[4]} 20 | } 21 | } 22 | end 23 | 24 | -- Functions to convert table to string 25 | -- Produces a compact, uncluttered representation of a table. Mutual recursion is employed 26 | -- http://lua-users.org/wiki/TableUtils 27 | function table.val_to_str ( v ) 28 | if "string" == type( v ) then 29 | v = string.gsub( v, "\n", "\\n" ) 30 | if string.match( string.gsub(v,"[^'\"]",""), '^"+$' ) then 31 | return "'" .. v .. "'" 32 | end 33 | return '"' .. string.gsub(v,'"', '\\"' ) .. '"' 34 | else 35 | return "table" == type( v ) and table.tostring( v ) or 36 | tostring( v ) 37 | end 38 | end 39 | 40 | function table.key_to_str ( k ) 41 | if "string" == type( k ) and string.match( k, "^[_%a][_%a%d]*$" ) then 42 | return k 43 | else 44 | return "[" .. table.val_to_str( k ) .. "]" 45 | end 46 | end 47 | 48 | function table.tostring( tbl ) 49 | local result, done = {}, {} 50 | for k, v in ipairs( tbl ) do 51 | table.insert( result, table.val_to_str( v ) ) 52 | done[ k ] = true 53 | end 54 | for k, v in pairs( tbl ) do 55 | if not done[ k ] then 56 | table.insert( result, 57 | table.key_to_str( k ) .. "=" .. table.val_to_str( v ) ) 58 | end 59 | end 60 | return "{" .. table.concat( result, "," ) .. "}" 61 | end 62 | 63 | if (sim_call_type==sim_childscriptcall_initialization) then 64 | -- The child script initialization 65 | objectHandle=simGetObjectAssociatedWithScript(sim_handle_self) 66 | objectName=simGetObjectName(objectHandle) 67 | -- Check if the required RosInterface is there: 68 | moduleName=0 69 | index=0 70 | rosInterfacePresent=false 71 | while moduleName do 72 | moduleName=simGetModuleName(index) 73 | if (moduleName=='RosInterface') then 74 | rosInterfacePresent=true 75 | end 76 | index=index+1 77 | end 78 | 79 | -- Prepare the float32 publisher and subscriber (we subscribe to the topic we advertise): 80 | if rosInterfacePresent then 81 | publisher=simExtRosInterface_advertise('/simulationTime','std_msgs/Float32') 82 | force_publisher=simExtRosInterface_advertise('/nico_feet_forces','std_msgs/String') 83 | subscriber=simExtRosInterface_subscribe('/simulationTime','std_msgs/Float32','subscriber_callback') 84 | end 85 | 86 | -- Get the graph handle 87 | if (simGetScriptExecutionCount()==0) then 88 | graphHandle=simGetObjectHandle("right_foot_avg_graph") 89 | end 90 | end 91 | 92 | if (sim_call_type==sim_childscriptcall_actuation) then 93 | -- Send an updated simulation time message, and send the transform of the object attached to this script: 94 | if rosInterfacePresent then 95 | simExtRosInterface_publish(publisher,{data=simGetSimulationTime()}) 96 | 97 | -- Read the forces acting on the left leg 98 | l1_handle=simGetObjectHandle('left_sensor_1') 99 | l1_result, l1_forceVector, l1_torqueVector=simReadForceSensor(l1_handle) 100 | l2_handle=simGetObjectHandle('left_sensor_2') 101 | l2_result, l2_forceVector, l2_torqueVector=simReadForceSensor(l2_handle) 102 | l3_handle=simGetObjectHandle('left_sensor_3') 103 | l3_result, l3_forceVector, l3_torqueVector=simReadForceSensor(l3_handle) 104 | l4_handle=simGetObjectHandle('left_sensor_4') 105 | l4_result, l4_forceVector, l4_torqueVector=simReadForceSensor(l4_handle) 106 | 107 | -- Read the forces acting on the right leg 108 | r1_handle=simGetObjectHandle('right_sensor_1') 109 | r1_result, r1_forceVector, r1_torqueVector=simReadForceSensor(r1_handle) 110 | r2_handle=simGetObjectHandle('right_sensor_2') 111 | r2_result, r2_forceVector, r2_torqueVector=simReadForceSensor(r2_handle) 112 | r3_handle=simGetObjectHandle('right_sensor_3') 113 | r3_result, r3_forceVector, r3_torqueVector=simReadForceSensor(r3_handle) 114 | r4_handle=simGetObjectHandle('right_sensor_4') 115 | r4_result, r4_forceVector, r4_torqueVector=simReadForceSensor(r4_handle) 116 | 117 | -- Concatenate all the force arrays into a single string 118 | force_str = table.val_to_str(l1_forceVector)..','.. 119 | table.val_to_str(l2_forceVector)..','.. 120 | table.val_to_str(l3_forceVector)..','.. 121 | table.val_to_str(l4_forceVector)..','.. 122 | table.val_to_str(r1_forceVector)..','.. 123 | table.val_to_str(r2_forceVector)..','.. 124 | table.val_to_str(r3_forceVector)..','.. 125 | table.val_to_str(r4_forceVector) 126 | 127 | if l1_forceVector and l2_forceVector and l3_forceVector and l4_forceVector and r1_forceVector and r2_forceVector and r3_forceVector and r4_forceVector then 128 | simAddStatusbarMessage('Force: '..force_str) 129 | simAddStatusbarMessage('Force2: '..l1_forceVector[1]) 130 | simExtRosInterface_publish(force_publisher,{data=force_str}) 131 | 132 | -- Compute the average of the left z-forces and the average of the right z-forces 133 | -- This is used for plotting 134 | right_z_avg = (r1_forceVector[3] + r2_forceVector[3] + r3_forceVector[3] + r4_forceVector[3])/4.0 135 | right_heel_z_avg = (r3_forceVector[3] + r4_forceVector[3])/2.0 136 | right_toe_z_avg = (r1_forceVector[3] + r2_forceVector[3])/2.0 137 | left_z_avg = (l1_forceVector[3] + l2_forceVector[3] + l3_forceVector[3] + l4_forceVector[3])/4.0 138 | left_heel_z_avg = (l3_forceVector[3] + l4_forceVector[3])/2.0 139 | left_toe_z_avg = (l1_forceVector[3] + l2_forceVector[3])/2.0 140 | 141 | -- Set the average values in the appropriate graph 142 | simSetGraphUserData(graphHandle, "r_heel_avg", right_heel_z_avg) 143 | simSetGraphUserData(graphHandle, "r_toe_avg", right_toe_z_avg) 144 | simSetGraphUserData(graphHandle, "r_avg", right_z_avg) 145 | simAddStatusbarMessage('Avg right heel (z): '..right_heel_z_avg) 146 | end 147 | simExtRosInterface_sendTransform(getTransformStamped(objectHandle,objectName,-1,'world')) 148 | -- To send several transforms at once, use simExtRosInterface_sendTransforms instead 149 | end 150 | end 151 | 152 | if (sim_call_type==sim_childscriptcall_cleanup) then 153 | -- Following not really needed in a simulation script (i.e. automatically shut down at simulation end): 154 | if rosInterfacePresent then 155 | simExtRosInterface_shutdownPublisher(publisher) 156 | simExtRosInterface_shutdownSubscriber(subscriber) 157 | end 158 | end 159 | --------------------------------------------------------------------------------