├── ev ├── __init__.py ├── show.py ├── fakedb.py ├── pypycompat.py ├── main.py ├── environment.py ├── db.py ├── universe.py └── creature.py ├── model ├── __init__.py ├── src │ ├── Makefile │ ├── qrmod.h │ └── qrmod.c ├── compile.py └── quadcopter.py ├── plotter ├── __init__.py └── quadplotter.py ├── testing ├── __init__.py ├── test_db.py ├── test_model.py └── test_universe.py ├── quadcopter.png ├── links.txt ├── README.md ├── LICENSE ├── .gitignore └── main.py /ev/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /model/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /plotter/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /testing/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /quadcopter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/antocuni/evolvingcopter/HEAD/quadcopter.png -------------------------------------------------------------------------------- /model/src/Makefile: -------------------------------------------------------------------------------- 1 | CFLAGS = -W -Wall -O2 2 | LDLIBS = -lm 3 | 4 | qrmod.so: 5 | gcc -o qrmod.so -shared -fPIC $(CFLAGS) qrmod.c $(LDLIBS) 6 | 7 | qrmod.o: qrsim.h qrmod.c 8 | gcc -c $(CFLAGS) qrmod.c 9 | -------------------------------------------------------------------------------- /links.txt: -------------------------------------------------------------------------------- 1 | Random useful links: 2 | 3 | Quadcopter Dynamics and Simulation 4 | http://andrew.gibiansky.com/blog/physics/quadcopter-dynamics/ 5 | 6 | How does a Quadrotor fly? A journey from physics, mathematics, control systems 7 | and computer science towards a "Controllable Flying Object" 8 | https://www.slideshare.net/corradosantoro/quadcopter-31045379 9 | 10 | Control of a Quadcopter 11 | https://www.slideshare.net/PabloGarciaAu/control-of-a-quadcopter 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # evolvingcopter 2 | 3 | (so far) Unsuccessful attempt to use evolutionary algorithms to drive a quadcopter. 4 | 5 | ![Quadcopter simulator](/quadcopter.png?raw=true) 6 | 7 | 8 | The code in the following directories has been copied and adapted from 9 | external repositories. The license for this code is the same as its original 10 | one, and the copyright of the original authors 11 | 12 | - `model/src/`: https://github.com/nzjrs/simple-quadrotor-simulator 13 | -------------------------------------------------------------------------------- /ev/show.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy 3 | import cPickle as pickle 4 | import ev.pypycompat 5 | from ev.environment import Environment 6 | 7 | import ev.creature 8 | sys.modules['evolution.creature'] = ev.creature 9 | 10 | def main(): 11 | # don't use scientific notation when printing 12 | numpy.set_printoptions(suppress=True) 13 | 14 | filename = sys.argv[1] 15 | with open(filename) as f: 16 | c = pickle.load(f) 17 | c.reset() 18 | 19 | print 'Matrix' 20 | print c.matrix 21 | print 22 | print 'Constant' 23 | print c.constant 24 | 25 | #env = Environment(show=True, z1=5, z2=3) 26 | #env = Environment(show=True, z1=5, z2=8) 27 | env = Environment(z0=100+3, z1=100+10, total_t=4, show=True) 28 | 29 | fitness = env.run(c) 30 | print 'fitness:', fitness 31 | 32 | if __name__ == '__main__': 33 | main() 34 | -------------------------------------------------------------------------------- /ev/fakedb.py: -------------------------------------------------------------------------------- 1 | class FakeTransaction(object): 2 | def __enter__(self): 3 | pass 4 | 5 | def __exit__(self, *args): 6 | pass 7 | 8 | class FakeDB(object): 9 | 10 | def __init__(self): 11 | self.lastid = 0 12 | self.generation = 0 13 | 14 | def count(self): 15 | return 0 16 | 17 | atomic = FakeTransaction() 18 | 19 | def new_generation(self): 20 | self.generation += 1 21 | 22 | def new(self, c): 23 | self.lastid += 1 24 | c.id = self.lastid 25 | c.born_at = self.generation 26 | return c 27 | 28 | def get_fitness(self, c): 29 | return getattr(c, 'fitness', None) 30 | 31 | def update_fitness(self, c, fitness): 32 | c.fitness = fitness 33 | 34 | def kill(self, c): 35 | c.killed = True 36 | 37 | def is_alive(self, c): 38 | killed = getattr(c, 'killed', False) 39 | return not killed 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 antocuni 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /model/compile.py: -------------------------------------------------------------------------------- 1 | from cffi import FFI 2 | ffibuilder = FFI() 3 | 4 | ffibuilder.set_source( 5 | "_qrmod", 6 | '#include "src/qrmod.h"', 7 | sources=['src/qrmod.c'], 8 | include_dirs=['.'], 9 | ) 10 | 11 | ffibuilder.cdef(""" 12 | typedef struct { 13 | double t; /* simulation time */ 14 | double phi; /* roll angle (Euler angle x) */ 15 | double theta; /* pitch angle (Euler angle y) */ 16 | double psi; /* yaw angle (Euler angle z) */ 17 | double x; /* position coordinate (earth axis x) */ 18 | double y; /* position coordinate (earth axis y) */ 19 | double z; /* position coordinate (earth axis z) */ 20 | 21 | double a1; /* rotor 1 control */ 22 | double a2; /* rotor 2 control */ 23 | double a3; /* rotor 3 control */ 24 | double a4; /* rotor 4 control */ 25 | 26 | ...; 27 | } qrstate_t; 28 | 29 | void qr_init(qrstate_t *qrstate, double z_at_gnd, double mass, 30 | double motor_thrust); 31 | void qr_nextstate(qrstate_t *qrstate, double DT); 32 | """) 33 | 34 | if __name__ == "__main__": 35 | ffibuilder.compile(verbose=True) 36 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | model/_qrmod.c 2 | model/_qrmod.o 3 | model/src/qrmod.o 4 | pdf/ 5 | 6 | # Byte-compiled / optimized / DLL files 7 | __pycache__/ 8 | *.py[cod] 9 | *$py.class 10 | 11 | # C extensions 12 | *.so 13 | 14 | # Distribution / packaging 15 | .Python 16 | env/ 17 | build/ 18 | develop-eggs/ 19 | dist/ 20 | downloads/ 21 | eggs/ 22 | .eggs/ 23 | lib/ 24 | lib64/ 25 | parts/ 26 | sdist/ 27 | var/ 28 | *.egg-info/ 29 | .installed.cfg 30 | *.egg 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .coverage 46 | .coverage.* 47 | .cache 48 | nosetests.xml 49 | coverage.xml 50 | *,cover 51 | .hypothesis/ 52 | 53 | # Translations 54 | *.mo 55 | *.pot 56 | 57 | # Django stuff: 58 | *.log 59 | local_settings.py 60 | 61 | # Flask stuff: 62 | instance/ 63 | .webassets-cache 64 | 65 | # Scrapy stuff: 66 | .scrapy 67 | 68 | # Sphinx documentation 69 | docs/_build/ 70 | 71 | # PyBuilder 72 | target/ 73 | 74 | # IPython Notebook 75 | .ipynb_checkpoints 76 | 77 | # pyenv 78 | .python-version 79 | 80 | # celery beat schedule file 81 | celerybeat-schedule 82 | 83 | # dotenv 84 | .env 85 | 86 | # virtualenv 87 | venv/ 88 | ENV/ 89 | 90 | # Spyder project settings 91 | .spyderproject 92 | 93 | # Rope project settings 94 | .ropeproject 95 | -------------------------------------------------------------------------------- /model/quadcopter.py: -------------------------------------------------------------------------------- 1 | from collections import namedtuple 2 | from model import _qrmod 3 | 4 | Point = namedtuple('Point', ['x', 'y', 'z']) 5 | RPY = namedtuple('RPY', ['roll', 'pitch', 'yaw']) 6 | 7 | class Quadcopter(object): 8 | 9 | def __init__(self, mass=1.0, motor_thrust=1.0): 10 | self.mass = mass 11 | self.motor_thrust = motor_thrust 12 | self.reset() 13 | 14 | def reset(self): 15 | self.qr = _qrmod.ffi.new('qrstate_t*') 16 | _qrmod.lib.qr_init(self.qr, 0, self.mass, self.motor_thrust) 17 | 18 | def run(self, t, dt=0.001): 19 | elapsed = 0 20 | while elapsed < t: 21 | self.step(dt) 22 | elapsed += dt 23 | 24 | def step(self, dt): 25 | _qrmod.lib.qr_nextstate(self.qr, dt) 26 | 27 | @property 28 | def t(self): 29 | return self.qr.t 30 | 31 | @property 32 | def position(self): 33 | # qrmod has a Z axis which goes "down". Invert it so that it goes "up" 34 | return Point(self.qr.x, self.qr.y, -self.qr.z) 35 | 36 | @position.setter 37 | def position(self, pos): 38 | x, y, z = pos 39 | self.qr.x = x 40 | self.qr.y = y 41 | self.qr.z = -z # invert the z 42 | 43 | @property 44 | def rpy(self): 45 | return RPY(self.qr.phi, self.qr.theta, self.qr.psi) 46 | 47 | def set_thrust(self, a1, a2, a3, a4): 48 | self.qr.a1 = max(0, a1) 49 | self.qr.a2 = max(0, a2) 50 | self.qr.a3 = max(0, a3) 51 | self.qr.a4 = max(0, a4) 52 | -------------------------------------------------------------------------------- /ev/pypycompat.py: -------------------------------------------------------------------------------- 1 | import sys 2 | IS_PYPY = hasattr(sys, 'pypy_version_info') 3 | if IS_PYPY: 4 | if '--no-numpypy' in sys.argv: 5 | print 'numpypy switched off by command line option' 6 | USE_NUMPYPY = False 7 | else: 8 | USE_NUMPYPY = True 9 | else: 10 | # CPython 11 | USE_NUMPYPY = False 12 | 13 | if USE_NUMPYPY: 14 | # make sure that 'import numpy' imports _numpypy.multiarray 15 | import random 16 | import _numpypy 17 | sys.modules['numpy'] = _numpypy.multiarray 18 | np = _numpypy.multiarray 19 | 20 | class np_random(object): 21 | 22 | @staticmethod 23 | def random(n): 24 | result = np.empty(n) 25 | values = result.ravel() 26 | for i in range(len(values)): 27 | values[i] = random.random() 28 | return result 29 | 30 | @staticmethod 31 | def normal(mu, sigma, n): 32 | result = np.empty(n) 33 | values = result.ravel() 34 | for i in range(len(values)): 35 | values[i] = random.gauss(mu, sigma) 36 | return result 37 | 38 | np.random = np_random 39 | 40 | else: 41 | # make it possible to unpickle numpypy arrays in CPython 42 | import numpy 43 | class fake_numpypy(object): 44 | 45 | @staticmethod 46 | def _reconstruct(t, shape, dtype): 47 | return numpy.empty(shape, dtype=dtype) 48 | 49 | @staticmethod 50 | def scalar(dtype, buf): 51 | arr = numpy.frombuffer(buf, dtype=dtype) 52 | return arr[0] 53 | 54 | sys.modules['_numpypy.multiarray'] = fake_numpypy 55 | -------------------------------------------------------------------------------- /testing/test_db.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import numpy as np 3 | from ev.db import CreatureDB 4 | 5 | @pytest.fixture 6 | def db(): 7 | return CreatureDB(":memory:") 8 | 9 | class FakeCreature(object): 10 | 11 | def __init__(self, data): 12 | self.id = None 13 | self.data = data 14 | 15 | 16 | class TestDB(object): 17 | 18 | def test_new_load(self, db): 19 | c1 = FakeCreature('hello') 20 | c2 = FakeCreature('world') 21 | db.new(c1) 22 | db.new(c2) 23 | assert c1.id == 1 24 | assert c2.id == 2 25 | # 26 | x1 = db.load(1) 27 | assert x1.id == 1 28 | assert x1.data == 'hello' 29 | # 30 | x2 = db.load(2) 31 | assert x2.id == 2 32 | assert x2.data == 'world' 33 | 34 | def test_update_fitness(self, db): 35 | c1 = FakeCreature('hello') 36 | c2 = FakeCreature('world') 37 | db.new(c1) 38 | db.new(c2) 39 | db.update_fitness(c1, 10) 40 | db.update_fitness(c2, 100) 41 | rows = db.load_all() 42 | assert rows == [ 43 | (1, 0, None, 10), 44 | (2, 0, None, 100), 45 | ] 46 | 47 | def test_generation(self, db): 48 | c1 = FakeCreature('hello') 49 | c2 = FakeCreature('world') 50 | db.new(c1) 51 | db.new_generation() 52 | db.new(c2) 53 | rows = db.load_all() 54 | assert rows == [ 55 | (1, 0, None, None), # generation 0 56 | (2, 1, None, None), # generation 1 57 | ] 58 | 59 | def test_kill(self, db): 60 | c1 = FakeCreature('hello') 61 | c2 = FakeCreature('world') 62 | db.new(c1) 63 | db.new(c2) 64 | assert db.is_alive(c1) 65 | assert db.is_alive(c2) 66 | db.kill(c2) 67 | assert db.is_alive(c1) 68 | assert not db.is_alive(c2) 69 | -------------------------------------------------------------------------------- /testing/test_model.py: -------------------------------------------------------------------------------- 1 | from pytest import approx 2 | from model import _qrmod 3 | from model.quadcopter import Quadcopter 4 | 5 | def test_qrmod(): 6 | st = _qrmod.ffi.new('qrstate_t*') 7 | _qrmod.lib.qr_init(st, 0, 1, 1) 8 | assert st.t == 0 9 | _qrmod.lib.qr_nextstate(st, 0.5) 10 | assert st.t == 0.5 11 | 12 | class TestQuadcopter(object): 13 | 14 | def test_run_reset(self): 15 | quad = Quadcopter() 16 | assert quad.t == 0 17 | quad.run(t=1) 18 | assert quad.t == approx(1) 19 | quad.reset() 20 | assert quad.t == 0 21 | 22 | def test_lift(self): 23 | quad = Quadcopter(mass=0.5, motor_thrust=0.5) 24 | assert quad.position == (0, 0, 0) 25 | assert quad.rpy == (0, 0, 0) 26 | # 27 | # power all the motors, to lift the quad vertically. The motors give a 28 | # total acceleration of 4g. Considering the gravity, we have a total 29 | # net acceleration of 3g. 30 | t = 1 # second 31 | g = 9.81 # m/s**2 32 | z = 0.5 * (3*g) * t**2 # d = 1/2 * a * t**2 33 | # 34 | quad.set_thrust(1, 1, 1, 1) 35 | quad.run(t=1, dt=0.0001) 36 | pos = quad.position 37 | assert pos.x == 0 38 | assert pos.y == 0 39 | assert pos.z == approx(z, rel=1e-3) # the simulated z is a bit 40 | # different than the computed one 41 | assert quad.rpy == (0, 0, 0) 42 | 43 | def test_Quadcopter_yaw(self): 44 | quad = Quadcopter() 45 | # only two motors on, to produce a yaw 46 | quad.set_thrust(0, 10, 0, 10) 47 | quad.run(t=1) 48 | assert quad.rpy.yaw > 0 49 | assert quad.rpy.pitch == 0 50 | assert quad.rpy.roll == 0 51 | # 52 | # now try to yaw in the opposite direction 53 | quad.reset() 54 | quad.set_thrust(10, 0, 10, 0) 55 | quad.run(t=1) 56 | assert quad.rpy.yaw < 0 57 | assert quad.rpy.pitch == 0 58 | assert quad.rpy.roll == 0 59 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from model.quadcopter import Quadcopter 3 | from plotter.quadplotter import QuadPlotter, RED, YELLOW 4 | 5 | def P_controller(quad, sp_z): 6 | """ 7 | a very simple "P-only" controller 8 | """ 9 | F = 0.5 # PWM needed to contrast the gravity 10 | K = 0.1 11 | diff = sp_z - quad.position.z 12 | T = F + K*diff 13 | quad.set_thrust(T, T, T, T) 14 | 15 | def lift_controller(quad): 16 | """ 17 | make the quad lift vertically 18 | """ 19 | T = 0.5 # PWM needed to contrast the gravity 20 | quad.set_thrust(T, T, T, T) 21 | 22 | def yaw_controller(quad): 23 | """ 24 | make the quad rotate 25 | """ 26 | T = 0.5 27 | d = T*0.1 28 | quad.set_thrust(T-d, T+d, T-d, T+d) 29 | 30 | 31 | class PD(object): 32 | 33 | def __init__(self, kp=0.2, kd=0.1): 34 | self.kp = kp 35 | self.kd = kd 36 | self.last_error = None 37 | 38 | def __call__(self, quad, dt, sp_z): 39 | F = 0.5 # PWM to contrast the gravity 40 | z = quad.position.z 41 | error = sp_z - z 42 | if self.last_error is None: 43 | d_error = 0 44 | else: 45 | d_error = (error - self.last_error) / dt 46 | # 47 | self.last_error = error 48 | diff = self.kp*error + self.kd*d_error 49 | T = F+diff 50 | quad.set_thrust(T, T, T, T) 51 | 52 | 53 | def main(): 54 | quad = Quadcopter() 55 | plotter = QuadPlotter() 56 | pd_controller = PD() 57 | 58 | quad.position = (0, 0, 3) # put it a bit above the ground 59 | dt = 0.01 60 | plotter.add_marker((0, 0, 5), RED) 61 | plotter.add_marker((0, 0, 3), YELLOW) 62 | setpoint = 5 63 | while plotter.show_step(): 64 | #P_controller(quad, sp_z=setpoint) 65 | pd_controller(quad, dt, sp_z=setpoint) 66 | #lift_controller(quad) 67 | #yaw_controller(quad) 68 | quad.step(dt) 69 | if quad.t > 3: 70 | setpoint = 3 71 | plotter.update(quad) 72 | 73 | 74 | 75 | if __name__ == '__main__': 76 | main() 77 | -------------------------------------------------------------------------------- /ev/main.py: -------------------------------------------------------------------------------- 1 | """ 2 | Usage: main [options] 3 | 4 | Build a population of creatures and evolve them to adapt to the given 5 | environment. 6 | 7 | Options: 8 | -n N Initial population count [Default: 500] 9 | --no-numpypy Force using the standard numpy even on PyPy 10 | --no-specialized Don't use the faster SpecializedCreature 11 | """ 12 | 13 | import time 14 | import sys 15 | import docopt 16 | import ev.pypycompat 17 | from ev.creature import Creature 18 | from ev.environment import Environment 19 | from ev.universe import Universe 20 | 21 | DB = 'creatures.db' 22 | 23 | def main(): 24 | args = docopt.docopt(__doc__) 25 | population = int(args['-n']) 26 | no_specialized = args['--no-specialized'] 27 | # 28 | # use envs in which z1-z0 is similar, but their absolute value is very 29 | # different, to avoid too much hard-coding in the matrices 30 | envs = [Environment(z0=3, z1=5), 31 | Environment(z0=5, z1=3), 32 | Environment(z0=100, z1=102), 33 | Environment(z0=302, z1=300), 34 | Environment(z0=50, z1=60, total_t=4)] 35 | uni = Universe(DB, envs, population=population, 36 | no_specialized=no_specialized) 37 | while True: 38 | try: 39 | a = time.time() 40 | uni.run_one_generation() 41 | b = time.time() 42 | elapsed = b-a 43 | print ('Generation %3d: min =%9.2f avg =%9.2f ' 44 | 'max =%9.2f [population = %d] [%.2f secs]' % ( 45 | uni.db.generation, 46 | uni.last_min, 47 | uni.last_avg, 48 | uni.last_max, 49 | len(uni.alive), 50 | elapsed)) 51 | except KeyboardInterrupt: 52 | print 53 | print 'Saving the best so far...' 54 | uni.save_best() 55 | print 'Press CTRL-C in the next 2 seconds to exit' 56 | try: 57 | time.sleep(2) 58 | except KeyboardInterrupt: 59 | print 60 | break 61 | 62 | if __name__ == '__main__': 63 | main() 64 | -------------------------------------------------------------------------------- /model/src/qrmod.h: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------- 2 | * 3 | * qrmod -- simple quad rotor model 4 | * Arjan van Gemund 5 | * Embedded Software Lab, TU Delft 6 | * 7 | *---------------------------------------------------------------------------- 8 | */ 9 | 10 | #ifndef _QR_MOD_H_ 11 | #define _QR_MOD_H_ 12 | 13 | typedef struct { 14 | unsigned char enable_gravity; 15 | unsigned char verbose; 16 | unsigned char pause; 17 | } qrcontrol_t; 18 | 19 | /* QR state 20 | */ 21 | typedef struct { 22 | 23 | /* configurable parameters */ 24 | double z_at_gnd; /* position coordinate (earth axis z) */ 25 | double mass; /* Kg */ 26 | double motor_thrust; /* Kgf (Kg-force) */ 27 | 28 | double t; /* simulation time */ 29 | 30 | /* quad rotor 31 | */ 32 | double phi; /* roll angle (Euler angle x) */ 33 | double theta; /* pitch angle (Euler angle y) */ 34 | double psi; /* yaw angle (Euler angle z) */ 35 | double x; /* position coordinate (earth axis x) */ 36 | double y; /* position coordinate (earth axis y) */ 37 | double z; /* position coordinate (earth axis z) */ 38 | double p; /* angular rotation speed phi (body axis x) */ 39 | double q; /* angular rotation speed theta (body axis y) */ 40 | double r; /* angular rotation speed psi (body axis z) */ 41 | double u; /* airspeed (body axis x) */ 42 | double v; /* airspeed (body axis y) */ 43 | double w; /* airspeed (body axis z) */ 44 | double mx; 45 | double my; 46 | 47 | /* actuators 48 | */ 49 | double a1; /* rotor 1 control */ 50 | double a2; /* rotor 2 control */ 51 | double a3; /* rotor 3 control */ 52 | double a4; /* rotor 4 control */ 53 | 54 | int leds; /* qr leds */ 55 | double tleds; /* last time blinked */ 56 | 57 | /* sensors 58 | */ 59 | double sp; /* gyro measrement of p */ 60 | double sq; /* gyro measrement of q */ 61 | double sr; /* gyro measrement of r */ 62 | double sx; /* acceleration (body axis x) */ 63 | double sy; /* acceleration (body axis y) */ 64 | double sz; /* acceleration (body axis z) */ 65 | 66 | qrcontrol_t sim_control; 67 | 68 | } qrstate_t; 69 | 70 | extern void qr_init(qrstate_t *qrstate, double z_at_gnd, double mass, 71 | double motor_thrust); 72 | extern void update_euler(qrstate_t *qrstate, double p, double q, double r, int use_body_angles); 73 | extern void qr_nextstate(qrstate_t *qrstate, double DT); 74 | 75 | #endif /* _QR_MOD_H_ */ 76 | -------------------------------------------------------------------------------- /ev/environment.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from model.quadcopter import Quadcopter 4 | 5 | class Environment(object): 6 | """ 7 | Very simple environment. The quadcopter starts at position (0, 0, 3). 8 | 9 | The goal is to reach setpoint z=5 in 2 seconds (so, they need to "go up"), 10 | then to reach setpoint z=3 in other 2 seconds (so, "go down"). 11 | 12 | Fitness is computed as to cumulative distance from the setpoint at each 13 | step 14 | """ 15 | 16 | def __init__(self, z0, z1, total_t=2, dt=0.01, show=False): 17 | self.total_t = total_t 18 | self.z0 = z0 19 | self.z1 = z1 20 | self.dt = dt 21 | self.show = show 22 | self.prev_distance = float('-inf') 23 | self.plotter = None 24 | if self.show: 25 | from plotter.quadplotter import QuadPlotter, RED, GREEN 26 | self.plotter = QuadPlotter() 27 | self.plotter.add_marker((0, 0, z1), RED) 28 | 29 | def run(self, creature): 30 | quad = Quadcopter() 31 | quad.position = (0, 0, self.z0) 32 | z_setpoint = self.z1 # first task: go to setpoint (0, 0, z1) 33 | fitness = 0 34 | while quad.t < self.total_t: 35 | ## if quad.t >= 2: 36 | ## # switch to second task 37 | ## z_setpoint = self.z2 38 | # 39 | inputs = [z_setpoint, quad.position.z] 40 | outputs = creature.run_step(inputs) 41 | assert len(outputs) == 1 42 | pwm = outputs[0] 43 | quad.set_thrust(pwm, pwm, pwm, pwm) 44 | quad.step(self.dt) 45 | fitness += self.compute_fitness(quad, z_setpoint) 46 | self.show_step(quad) 47 | return fitness 48 | 49 | def show_step(self, quad): 50 | if self.show: 51 | self.plotter.update(quad) 52 | self.plotter.show_step() 53 | 54 | def compute_fitness(self, quad, z_setpoint): 55 | # for now, the goal is to reach the target position as fast as 56 | # possible and then to stay there. So a measure of the fitness is the 57 | # distance to the target at every step (the goal is to *minimize* the 58 | # total value, of course) 59 | x0, y0, z0 = 0, 0, z_setpoint 60 | x1, y1, z1 = quad.position 61 | distance = math.sqrt((x0-x1)**2 + (y0-y1)**2 + (z0-z1)**2) 62 | k = 1 63 | if distance > self.prev_distance: 64 | k = 3 # if we are overshooting, pay a penalty 65 | self.prev_distance = distance 66 | return distance * k 67 | -------------------------------------------------------------------------------- /testing/test_universe.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import random 3 | import itertools 4 | from ev.universe import Universe 5 | 6 | @pytest.fixture 7 | def filename(tmpdir): 8 | return str(tmpdir.join('creatures.db')) 9 | 10 | class FakeEnv(object): 11 | def run(self, c): 12 | return c.id 13 | 14 | def test_first_generation(filename): 15 | uni = Universe(filename, envs=None, population=10) 16 | assert len(uni.alive) == 10 17 | ids = [c.id for c in uni.alive] 18 | assert sorted(ids) == range(1, 11) 19 | for c in uni.alive: 20 | assert uni.db.get_fitness(c) is None 21 | 22 | 23 | def test_compute_fitness(filename): 24 | uni = Universe(filename, envs=[FakeEnv()], population=5) 25 | uni.compute_fitness() 26 | # 27 | for c in uni.alive: 28 | fitness = uni.db.get_fitness(c) 29 | assert fitness == c.id 30 | 31 | def test_compute_fitness_partial(filename): 32 | uni = Universe(filename, envs=[FakeEnv()], population=5) 33 | creatures = list(uni.alive) 34 | creatures.sort(key=lambda c: c.id) 35 | c1, c2, c3, c4, c5 = creatures 36 | # 37 | # manually compute the fitness for c1 and c2 38 | uni.db.update_fitness(c1, 10) 39 | uni.db.update_fitness(c2, 20) 40 | # 41 | computed = set() 42 | def compute_fitness_one(c): 43 | computed.add(c) 44 | return 1 45 | uni.compute_fitness_one = compute_fitness_one 46 | uni.compute_fitness() 47 | assert computed == set([c3, c4, c5]) 48 | 49 | 50 | def test_kill_some(filename, monkeypatch): 51 | def my_sample(population, k): 52 | # not really random :) 53 | population = list(population) 54 | population.sort(key=lambda c: c.id) 55 | return population[:k] 56 | monkeypatch.setattr(random, 'sample', my_sample) 57 | # 58 | uni = Universe(filename, envs=[FakeEnv()], population=100) 59 | all_creatures = list(uni.alive) 60 | all_creatures.sort(key=lambda c: c.id) 61 | uni.compute_fitness() # c1 is the best, c100 is the worst 62 | uni.kill_some() 63 | # 64 | # check that the expected creatures survived 65 | all_ids = range(1, 101) 66 | best = all_ids[:10] 67 | unlucky = all_ids[10:15] 68 | good = all_ids[15:50] # (excluding the unlucky ones) 69 | lucky = all_ids[50:55] 70 | bad = all_ids[55:] # (excluding the lucky ones) 71 | alive_ids = sorted([c.id for c in uni.alive]) 72 | assert alive_ids == (best + good + lucky) 73 | # 74 | # check that the DB agrees about what is alive and what it's not 75 | for c in all_creatures: 76 | should_be_alive = (c in uni.alive) 77 | assert uni.db.is_alive(c) == should_be_alive 78 | -------------------------------------------------------------------------------- /ev/db.py: -------------------------------------------------------------------------------- 1 | import sqlite3 2 | import cPickle as pickle 3 | 4 | class CreatureDB(object): 5 | 6 | def __init__(self, filename): 7 | self.conn = sqlite3.connect(filename, isolation_level=None) # autocommit 8 | self.conn.text_factory = str 9 | self.cur = self.conn.cursor() 10 | self.cur.execute(""" 11 | CREATE TABLE IF NOT EXISTS creatures ( 12 | id INTEGER PRIMARY KEY, 13 | born_at INTEGER, 14 | killed_at INTEGER, 15 | fitness REAL, 16 | pickled TEXT 17 | ) 18 | """) 19 | self.cur.execute("SELECT MAX(born_at) FROM creatures") 20 | self.generation = self.cur.fetchone()[0] 21 | if self.generation is None: 22 | self.generation = 0 23 | 24 | @property 25 | def atomic(self): 26 | return Transaction(self.cur) 27 | 28 | def new_generation(self): 29 | self.generation += 1 30 | 31 | def new(self, c): 32 | """ 33 | Insert a new creature in the DB, and set c.id accordingly 34 | """ 35 | assert c.id is None 36 | # create a new row in the DB, to generate an ID 37 | self.cur.execute("INSERT INTO creatures(id) VALUES(NULL)") 38 | c.id = self.cur.lastrowid 39 | born_at = self.generation 40 | pickled = pickle.dumps(c) 41 | # save the updated c 42 | self.cur.execute(""" 43 | UPDATE creatures 44 | SET born_at = ?, pickled = ? 45 | WHERE id = ? 46 | """, (born_at, pickled, c.id)) 47 | 48 | def load(self, id): 49 | self.cur.execute("SELECT pickled FROM creatures WHERE id=?", (id,)) 50 | pickled = self.cur.fetchone()[0] 51 | return pickle.loads(pickled) 52 | 53 | def update_fitness(self, c, fitness): 54 | assert c.id is not None 55 | self.cur.execute(""" 56 | UPDATE creatures 57 | SET fitness = ? 58 | WHERE id = ? 59 | """, (fitness, c.id)) 60 | 61 | def get_fitness(self, c): 62 | assert c.id is not None 63 | self.cur.execute("SELECT fitness FROM creatures WHERE id=?", (c.id,)) 64 | fitness = self.cur.fetchone()[0] 65 | return fitness 66 | 67 | def kill(self, c): 68 | assert c.id is not None 69 | killed_at = self.generation 70 | self.cur.execute(""" 71 | UPDATE creatures 72 | SET killed_at = ? 73 | WHERE id = ? 74 | """, (killed_at, c.id)) 75 | 76 | def is_alive(self, c): 77 | assert c.id is not None 78 | self.cur.execute("SELECT killed_at FROM creatures WHERE id=?", (c.id,)) 79 | killed_at = self.cur.fetchone()[0] 80 | return killed_at is None 81 | 82 | def count(self): 83 | self.cur.execute("SELECT COUNT(*) FROM creatures") 84 | return self.cur.fetchone()[0] 85 | 86 | def load_all(self): 87 | """ 88 | Return all the data *except* the creature itself 89 | """ 90 | self.cur.execute("SELECT id, born_at, killed_at, fitness FROM creatures") 91 | return self.cur.fetchall() 92 | 93 | def load_best(self): 94 | """ 95 | Return the best creature ever recorded 96 | """ 97 | self.cur.execute(""" 98 | SELECT id 99 | FROM creatures 100 | WHERE fitness == ( 101 | SELECT MIN(fitness) FROM creatures 102 | ) 103 | """) 104 | id = self.cur.fetchone()[0] 105 | return self.load(id) 106 | 107 | class Transaction(object): 108 | 109 | def __init__(self, cur): 110 | self.cur = cur 111 | 112 | def __enter__(self): 113 | self.cur.execute('BEGIN') 114 | return self 115 | 116 | def __exit__(self, etype, evalue, tb): 117 | if etype: 118 | self.cur.execute('ROLLBACK') 119 | else: 120 | self.cur.execute('COMMIT') 121 | -------------------------------------------------------------------------------- /plotter/quadplotter.py: -------------------------------------------------------------------------------- 1 | from pyqtgraph.Qt import QtCore, QtGui 2 | import pyqtgraph.opengl as gl 3 | import pyqtgraph as pg 4 | import numpy as np 5 | 6 | RED = (1, 0, 0) 7 | CYAN = (0, 1, 1) 8 | YELLOW = (1, 1, 0) 9 | GREEN = (0, 1, 0) 10 | 11 | 12 | class QuadPlotter(object): 13 | 14 | def __init__(self, title='Quadcopter', arm_length=2): 15 | self.arm_length = arm_length 16 | self.app = QtGui.QApplication([]) 17 | self._make_glview() 18 | self._make_layout() 19 | self.win.setWindowTitle(title) 20 | self.win.show() 21 | N = 3 22 | self.win.resize(640*N, 480*N) 23 | self.win.move(0, 0) 24 | 25 | def _make_layout(self): 26 | self.win = QtGui.QWidget() 27 | layout = QtGui.QGridLayout() 28 | self.win.setLayout(layout) 29 | # 30 | self.label_t = QtGui.QLabel() 31 | self.label_t.setAlignment(QtCore.Qt.AlignRight) 32 | self.glview.setSizePolicy(QtGui.QSizePolicy.Expanding, 33 | QtGui.QSizePolicy.Expanding) 34 | # 35 | layout.addWidget(self.glview, 0, 0) 36 | layout.addWidget(self.label_t, 1, 0) 37 | 38 | def _make_glview(self): 39 | self.glview = gl.GLViewWidget() 40 | self.glview.opts['distance'] = 40 41 | #self.glview.pan(dx=0, dy=0, dz=100) 42 | self._make_grid() 43 | self._make_quad() 44 | 45 | def _make_grid(self): 46 | # make the grid 47 | gx = gl.GLGridItem() 48 | gx.rotate(90, 0, 1, 0) 49 | gx.translate(-10, 0, 0) 50 | self.glview.addItem(gx) 51 | gy = gl.GLGridItem() 52 | gy.rotate(90, 1, 0, 0) 53 | gy.translate(0, -10, 0) 54 | self.glview.addItem(gy) 55 | gz = gl.GLGridItem() 56 | gz.translate(0, 0, -10) 57 | self.glview.addItem(gz) 58 | # 59 | # make the axes 60 | ax = gl.GLAxisItem() 61 | ax.setSize(100, 100, 100) 62 | self.glview.addItem(ax) 63 | 64 | def _make_quad(self): 65 | L = self.arm_length 66 | H = L/8.0 67 | lines = [ 68 | # from to color 69 | ([ -L, 0, 0], [-L+H, 0, 0], RED), # indicate the norh 70 | ([-L+H, 0, 0], [ L, 0, 0], CYAN), # north-south arm 71 | ([ 0, -L, 0], [ 0, L, 0], YELLOW), # west-east arm 72 | ([ 0, 0, 0], [ 0, 0, H], GREEN), 73 | ] 74 | points = [] 75 | colors = [] 76 | for a, b, color in lines: 77 | points.append(a) 78 | points.append(b) 79 | colors.append(color) 80 | colors.append(color) 81 | # 82 | self.quadplot = gl.GLLinePlotItem(pos=np.array(points), 83 | color=np.array(colors), 84 | mode='lines', 85 | width=3) 86 | self.glview.addItem(self.quadplot) 87 | 88 | def add_marker(self, pos, color, size=0.1): 89 | points = np.array([pos]) 90 | p = gl.GLScatterPlotItem(pos=points, 91 | color=np.array(color), 92 | size=size, 93 | pxMode=False) 94 | self.glview.addItem(p) 95 | 96 | def update(self, quad): 97 | """ 98 | Set the new position and rotation of the quadcopter: ``pos`` is a tuple 99 | (x, y, z), and rpy is a tuple (roll, pitch, yaw) expressed in 100 | *radians* 101 | """ 102 | x, y, z = quad.position 103 | roll, pitch, yaw = np.rad2deg(quad.rpy) 104 | self.quadplot.resetTransform() 105 | self.quadplot.translate(x, y, z) 106 | self.quadplot.rotate(roll, 1, 0, 0, local=True) 107 | self.quadplot.rotate(pitch, 0, 1, 0, local=True) 108 | self.quadplot.rotate(yaw, 0, 0, 1, local=True) 109 | # 110 | self.label_t.setText('t = %5.2f' % quad.t) 111 | 112 | def show_step(self, dt=0.01): 113 | if not self.win.isVisible(): 114 | return False 115 | self.app.processEvents(QtCore.QEventLoop.AllEvents, 0.01) 116 | return True 117 | 118 | def show(self): 119 | self.app.exec_() 120 | 121 | if __name__ == '__main__': 122 | import math 123 | x, y, z = 0, 0, 0 124 | roll, pitch, yaw = 0, 0, 0 125 | plot = QuadPlotter() 126 | while plot.show_step(): 127 | x += 0.01 128 | yaw = math.sin(x) 129 | plot.update(pos=(x, y, z), rpy=(roll, pitch, yaw)) 130 | -------------------------------------------------------------------------------- /ev/universe.py: -------------------------------------------------------------------------------- 1 | import random 2 | import numpy as np 3 | from ev.db import CreatureDB 4 | from ev.fakedb import FakeDB 5 | from ev.creature import Creature, SpecializedCreature 6 | from ev.environment import Environment 7 | 8 | class Universe(object): 9 | 10 | def __init__(self, filename, envs, population=500, no_specialized=False): 11 | #self.db = CreatureDB(filename) 12 | self.db = FakeDB() 13 | self.envs = envs 14 | self.population = population 15 | self.no_specialized = no_specialized 16 | self.alive = set() 17 | if self.db.count() == 0: 18 | # start from scratch 19 | self.make_first_generation() 20 | else: 21 | raise NotImplementedError('load from existing DB') 22 | 23 | def make_first_generation(self): 24 | print 'Creating first generation' 25 | if self.no_specialized: 26 | print 'WARNING: using the slow non-specialized creature' 27 | make_creature = Creature 28 | else: 29 | make_creature = SpecializedCreature 30 | # 31 | with self.db.atomic: 32 | for i in range(self.population): 33 | c = make_creature() 34 | self.db.new(c) 35 | self.alive.add(c) 36 | 37 | def compute_fitness(self): 38 | """ 39 | Make sure that we computed the fitness of all the alive creatures. 40 | """ 41 | # we check if the fitness has already been computed. This happens in 42 | # two cases: 43 | # 44 | # 1. at each generation, half of the population survived, and thus 45 | # we already know its fitness 46 | # 47 | # 2. in case we interrupted the simulation in the middle of 48 | # compute_fitness 49 | values = [] 50 | for c in self.alive: 51 | fitness = self.db.get_fitness(c) 52 | if fitness is None: 53 | fitness = self.compute_fitness_one(c) 54 | values.append(fitness) 55 | # 56 | self.last_min = min(values) 57 | self.last_avg = sum(values) / len(values) 58 | self.last_max = max(values) 59 | 60 | def compute_fitness_one(self, c): 61 | fitness = 0 62 | for env in self.envs: 63 | fitness += env.run(c) 64 | self.db.update_fitness(c, fitness) 65 | return fitness 66 | 67 | def kill_some(self): 68 | # first, sort the creatures by fitness (smaller is better) 69 | creatures = list(self.alive) 70 | creatures.sort(key=self.db.get_fitness) 71 | # 72 | # normally, only the upper half would survive. However, to improve 73 | # genetic variation, a random 5% surives even if it's in the lower 74 | # half. We do the following: 75 | # 76 | # 1. the best 10% always survives 77 | # 2. N creatures in the [10%:50%] are killed; the other survive 78 | # 3. N creatures in the [50%:100%] survives; the other are killed 79 | # 80 | N = int(round(self.population * 0.15)) # 15% survives randomly 81 | i10 = int(round(self.population * 0.01)) # save only the top 1% 82 | i50 = int(round(self.population * 0.5)) 83 | best = set(creatures[:i10]) 84 | good = set(creatures[i10:i50]) 85 | bad = set(creatures[i50:]) 86 | # 87 | unlucky = set(random.sample(good, N)) 88 | lucky = set(random.sample(bad, N)) 89 | good.difference_update(unlucky) 90 | bad.difference_update(lucky) 91 | survivors = best.union(good).union(lucky) 92 | killed = bad.union(unlucky) 93 | # 94 | assert len(survivors) + len(killed) == self.population 95 | for c in killed: 96 | self.db.kill(c) 97 | self.alive = survivors 98 | 99 | def reproduce(self): 100 | new_creatures = [c.reproduce() for c in self.alive] 101 | for c in new_creatures: 102 | self.db.new(c) 103 | self.alive.add(c) 104 | 105 | def run_one_generation(self): 106 | self.compute_fitness() 107 | self.kill_some() 108 | self.db.new_generation() 109 | self.reproduce() 110 | 111 | def save_best(self): 112 | import cPickle as pickle 113 | creatures = list(self.alive) 114 | creatures.sort(key=self.db.get_fitness) 115 | for c in creatures: 116 | fitness = self.db.get_fitness(c) 117 | if fitness is not None: 118 | best = c 119 | break 120 | else: 121 | print 'something is wrong' 122 | import pdb;pdb.set_trace() 123 | # 124 | filename = 'creature-%.2f-%d.pickle' % (fitness, best.id) 125 | with open(filename, 'w') as f: 126 | pickle.dump(best, f) 127 | print 'best creature saved to', filename 128 | 129 | -------------------------------------------------------------------------------- /ev/creature.py: -------------------------------------------------------------------------------- 1 | import random 2 | import numpy as np 3 | 4 | class Creature(object): 5 | INPUTS = 2 # z_setpoint, current z position 6 | OUTPUTS = 1 # PWM for all 4 motors 7 | STATE_VARS = 1 8 | 9 | def __init__(self, parent=None, matrix=None, constant=None): 10 | N = self.INPUTS + self.STATE_VARS 11 | M = self.OUTPUTS + self.STATE_VARS 12 | self.id = None 13 | # don't save the parent for now, else pickle.dump gives RecursionError 14 | #self.parent = parent 15 | self.matrix = matrix 16 | self.constant = constant 17 | # 18 | if self.matrix is None: 19 | self.matrix = np.random.random(N*M).reshape(M, N) - 0.5 20 | if self.constant is None: 21 | self.constant = np.random.random(M) - 0.5 22 | self.reset() 23 | 24 | def __repr__(self): 25 | return '' % (self.id,) 26 | 27 | def reset(self): 28 | self.state = np.zeros(self.STATE_VARS + self.INPUTS) 29 | 30 | def run_step(self, inputs): 31 | # state: [state_vars ... inputs] 32 | # out_values: [state_vars, ... outputs] 33 | self.state[self.STATE_VARS:] = inputs 34 | out_values = np.dot(self.matrix, self.state) + self.constant 35 | self.state[:self.STATE_VARS] = out_values[:self.STATE_VARS] 36 | outputs = out_values[self.STATE_VARS:] 37 | return outputs 38 | 39 | def reproduce(self): 40 | mutate_meths = [self._mutate_normal, 41 | self._mutate_random, 42 | self._mutate_one, 43 | self._mutate_all, 44 | self._mutate_zero_or_one, 45 | self._mutate_set_equal_to] 46 | mutate = random.choice(mutate_meths) 47 | matrix = self.matrix 48 | constant = self.constant 49 | if random.choice([True, False]): 50 | matrix = mutate(matrix) 51 | else: 52 | constant = mutate(constant) 53 | return self.__class__(parent=self, matrix=matrix, constant=constant) 54 | 55 | def _mutate_random(self, x): 56 | # change every value of a random k between 50% and 150% 57 | k = np.random.random(x.shape) + 0.5 58 | return x*k 59 | 60 | def _mutate_normal(self, x): 61 | # like mutate_random, but with a normal distribution 62 | mu = 0 63 | sigma = 0.1 64 | k = np.random.normal(mu, sigma, x.shape) + 0.5 65 | return x*k 66 | return self.matrix, new_constant 67 | 68 | def _mutate_one(self, x): 69 | # change only one item, between 50% and 150% 70 | shape = x.shape 71 | flat = x.flatten() 72 | i = random.randrange(len(flat)) 73 | k = random.random() + 0.5 74 | flat[i] *= k 75 | return flat.reshape(shape) 76 | 77 | def _mutate_all(self, x): 78 | # change all the items by the same k 79 | k = random.random() + 0.5 80 | return x*k 81 | 82 | def _mutate_zero_or_one(self, x): 83 | shape = x.shape 84 | flat = x.flatten() 85 | i = random.randrange(len(flat)) 86 | v = random.choice([0.0, 1.0]) 87 | flat[i] = v 88 | return flat.reshape(shape) 89 | 90 | def _mutate_set_equal_to(self, x): 91 | shape = x.shape 92 | flat = x.flatten() 93 | i = random.randrange(len(flat)) 94 | j = random.randrange(len(flat)) 95 | sign = random.choice([1, -1]) 96 | flat[i] = flat[j] * sign 97 | return flat.reshape(shape) 98 | 99 | 100 | class SpecializedCreature(Creature): 101 | """ 102 | This is equivalent to Creature if: 103 | INPUTS = 2 104 | OUTPUTS = 1 105 | STATE_VARS = 1 106 | 107 | but it unrolls the dot() product, so it is much faster 108 | """ 109 | 110 | def __init__(self, *args, **kwargs): 111 | Creature.__init__(self, *args, **kwargs) 112 | # store the data in a plain Python list, which pypy is able to 113 | # optimize as a float array 114 | self.data = list(self.matrix.ravel()) + list(self.constant) 115 | self.data_state = [0.0] 116 | assert self.matrix.shape == (2, 3) 117 | assert len(self.data) == 8 118 | 119 | def run_step(self, inputs): 120 | # state: [state_vars ... inputs] 121 | # out_values: [state_vars, ... outputs] 122 | k0, k1, k2, q0, q1, q2, c0, c1 = self.data 123 | s0 = self.data_state[0] 124 | z_sp, z = inputs 125 | # 126 | # compute the output 127 | out0 = s0*k0 + z_sp*k1 + z*k2 + c0 128 | out1 = s0*q0 + z_sp*q1 + z*q2 + c1 129 | # 130 | self.data_state[0] = out0 131 | outputs = [out1] 132 | # 133 | # sanity check 134 | ## expected_outputs = Creature.run_step(self, inputs) 135 | ## assert self.data_state[0] == self.state[0] 136 | ## assert expected_outputs == outputs 137 | # 138 | return outputs 139 | -------------------------------------------------------------------------------- /model/src/qrmod.c: -------------------------------------------------------------------------------- 1 | /*-------------------------------------------------------------------------- 2 | * 3 | * qrmod -- Simple quad rotor model 4 | * Arjan van Gemund 5 | * Embedded Software Lab, TU Delft 6 | * 7 | * modified by Antonio Cuni (anto.cuni@gmail.com), 2017 8 | * 9 | * Euler angles: NASA standard aeroplane (psi-theta-phi) 10 | * 11 | *-------------------------------------------------------------------------- 12 | */ 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "qrmod.h" 19 | 20 | void qr_init(qrstate_t *qrstate, double z_at_gnd, double mass, 21 | double motor_thrust) 22 | { 23 | qrstate->z_at_gnd = z_at_gnd; /* position coordinate (earth axis z) */ 24 | qrstate->mass = mass; 25 | qrstate->motor_thrust = motor_thrust; 26 | 27 | qrstate->t = 0; /* simulation time */ 28 | 29 | qrstate->u = 0; /* airspeed (body axis x) */ 30 | qrstate->v = 0; /* airspeed (body axis y) */ 31 | qrstate->w = 0; /* airspeed (body axis z) */ 32 | qrstate->p = 0; /* angular rotation speed phi (body axis x) */ 33 | qrstate->q = 0; /* angular rotation speed theta (body axis y) */ 34 | qrstate->r = 0; /* angular rotation speed psi (body axis z) */ 35 | qrstate->phi = 0; /* roll angle (body axis x) */ 36 | qrstate->theta = 0; /* pitch angle (body axis y) */ 37 | qrstate->psi = 0; /* yaw angle (body axis z) */ 38 | qrstate->x = 0; /* position coordinate (earth axis x) */ 39 | qrstate->y = 0; /* position coordinate (earth axis y) */ 40 | qrstate->z = z_at_gnd; /* position coordinate (earth axis z) */ 41 | qrstate->mx = 0; 42 | qrstate->my = 0; 43 | 44 | /* actuators: 45 | they are values between 0.0 and 1.0, as if they were PWM signal. The 46 | motors have a defined maximum thrust. The actual thrust is proportional 47 | to the square of PWM 48 | */ 49 | qrstate->a1 = 0; /* rotor 1 */ 50 | qrstate->a2 = 0; /* rotor 2 */ 51 | qrstate->a3 = 0; /* rotor 3 */ 52 | qrstate->a4 = 0; /* rotor 4 */ 53 | qrstate->leds = 0; /* reset leds */ 54 | qrstate->tleds = 0; /* reset led blink time */ 55 | 56 | /* sensors 57 | */ 58 | qrstate->sp = 0; /* gyro measrement of p */ 59 | qrstate->sq = 0; /* gyro measrement of q */ 60 | qrstate->sr = 0; /* gyro measrement of r */ 61 | qrstate->sx = 0; /* acceleration (body axis x) */ 62 | qrstate->sy = 0; /* acceleration (body axis y) */ 63 | qrstate->sz = 0; /* acceleration (body axis z) */ 64 | 65 | qrstate->sim_control.enable_gravity = 1; 66 | qrstate->sim_control.verbose = 0; 67 | qrstate->sim_control.pause = 0; 68 | } 69 | 70 | /*-------------------------------------------------------------------------- 71 | * update earth orientation (Euler angles) from body angle increments 72 | * [Etkin page 104] 73 | * 74 | * Euler angle update mode 75 | * use_body_angles = 1: angle increments in terms of body frame (-> xformation) 76 | * else: angle increments in terms of earth frame (-> addition) 77 | *-------------------------------------------------------------------------- 78 | */ 79 | void update_euler(qrstate_t *qrstate, 80 | double p, double q, double r, int use_body_angles) 81 | { 82 | double phi, theta, psi; 83 | double d_phi, d_theta, d_psi; 84 | 85 | phi = qrstate->phi; 86 | theta = qrstate->theta; 87 | psi = qrstate->psi; 88 | 89 | if (use_body_angles) { 90 | /* Body angles */ 91 | d_phi = p + (q * sin(phi) + r * cos(phi)) * tan(theta); 92 | d_theta = q * cos(phi) - r * sin(phi); 93 | d_psi = (q * sin(phi) + r * cos(phi)) / cos(theta); 94 | } 95 | else { 96 | /* Euler angles */ 97 | d_phi = p; 98 | d_theta = q; 99 | d_psi = r; 100 | } 101 | 102 | qrstate->phi += d_phi; 103 | qrstate->theta += d_theta; 104 | qrstate->psi += d_psi; 105 | } 106 | 107 | /*-------------------------------------------------------------------------- 108 | * quad rotor model with full dynamics and kinematics equations 109 | *-------------------------------------------------------------------------- 110 | */ 111 | void qr_nextstate(qrstate_t *qrstate, double DT) 112 | { 113 | double t, tleds; 114 | double a1, a2, a3, a4; 115 | int leds; 116 | double X, Y, Z; 117 | double L, M, N; 118 | double u, v, w, du, dv, dw; 119 | double p, q, r, dp, dq, dr; 120 | double phi, theta, psi, dphi, dtheta, dpsi; 121 | double x, y, z, dx, dy, dz; 122 | double sinphi, sintheta, sinpsi, cosphi, costheta, cospsi, tantheta; 123 | double a_, b_, c_, d_, e_, f_; 124 | 125 | double o1, o2, o3, o4; // rotor speed omega 126 | double mf, m, b, d, g; 127 | double Ix, Iy, Iz, Izx; 128 | 129 | /* gravity force 130 | */ 131 | if (qrstate->sim_control.enable_gravity) 132 | g = 9.81; /* 9.81; */ 133 | else 134 | g = 0; 135 | 136 | /* quad rotor constants (SI units) 137 | */ 138 | 139 | /* 140 | antocuni's notes, reading this pdf: 141 | https://www.slideshare.net/PabloGarciaAu/control-of-a-quadcopter 142 | 143 | section 2.2.3, propulsive moments: 144 | 145 | "d is the distance from the center of the propeller to the center of 146 | gravity": this is the *b* variable 147 | 148 | d[N] is a fictitious distance which represents the aerodynamic 149 | reaction of the propellers due to the draft of the blades: this is the 150 | *d* variable 151 | */ 152 | mf = qrstate->motor_thrust * g; // N, max force produced by each motor 153 | b = 1.0; // distance from the center of propeller to center of quad 154 | d = 10.0; // 10*b: avoid lots of Z thrust when yawing 155 | m = qrstate->mass; // mass 156 | Ix = Iy = 1.0; 157 | Iz = 2.0; 158 | Izx = 0.0; 159 | 160 | /* copy from struct 161 | */ 162 | t = qrstate->t; 163 | a1 = qrstate->a1; 164 | a2 = qrstate->a2; 165 | a3 = qrstate->a3; 166 | a4 = qrstate->a4; 167 | leds = qrstate->leds; 168 | tleds = qrstate->tleds; 169 | 170 | u = qrstate->u; 171 | v = qrstate->v; 172 | w = qrstate->w; 173 | p = qrstate->p; 174 | q = qrstate->q; 175 | r = qrstate->r; 176 | phi = qrstate->phi; 177 | theta = qrstate->theta; 178 | psi = qrstate->psi; 179 | x = qrstate->x; 180 | y = qrstate->y; 181 | z = qrstate->z; 182 | 183 | /* optimize a bit 184 | */ 185 | sinphi = sin(phi); sintheta = sin(theta); sinpsi = sin(psi); 186 | cosphi = cos(phi); costheta = cos(theta); cospsi = cos(psi); 187 | if (costheta == 0) { 188 | printf("singularity in Euler angles: cos(theta) = 0\n"); 189 | exit(0); 190 | } 191 | tantheta = sintheta / costheta; 192 | 193 | 194 | 195 | 196 | /* first part of the quad rotor model (specific): 197 | * convert actuators signals to forces and moments 198 | * (in terms of body axes) 199 | * 200 | * NOTE: OVERLY SIMPLE FOR NOW! 201 | */ 202 | 203 | 204 | /* clip rotor pwm 205 | */ 206 | if (a1 < 0) a1 = 0; 207 | if (a1 > 1) a1 = 1; 208 | if (a2 < 0) a2 = 0; 209 | if (a2 > 1) a2 = 1; 210 | if (a3 < 0) a3 = 0; 211 | if (a3 > 1) a3 = 1; 212 | if (a4 < 0) a4 = 0; 213 | if (a4 > 1) a4 = 1; 214 | 215 | /* compute rotor speed 216 | */ 217 | o1 = a1; // front, turning clockwise 218 | o2 = a2; // starboard, turning counter-clockwise 219 | o3 = a3; // aft, turning clockwise 220 | o4 = a4; // port, turning counter-clockwise 221 | 222 | /* compute longitudal thrust (body axis) 223 | */ 224 | X = 0; 225 | 226 | /* compute lateral thrust (body axis) 227 | */ 228 | Y = 0; 229 | 230 | /* compute vertical thrust (body axis) 231 | * function of 4 rotors 232 | */ 233 | Z = - mf * (o1*o1 + o2*o2 + o3*o3 + o4*o4); 234 | 235 | /* compute roll moment (body axis) 236 | */ 237 | L = b * mf * (o4*o4 - o2*o2); 238 | 239 | /* compute pitch moment (body axis) 240 | */ 241 | M = b * mf * (o1*o1 - o3*o3); 242 | 243 | /* compute yaw moment (body axis) 244 | */ 245 | N = d * mf * (o2*o2 + o4*o4 - o1*o1 - o3*o3); 246 | 247 | /* trace control data 248 | */ 249 | if (qrstate->sim_control.verbose) { 250 | printf("t a1 a2 a3 a4 X Y Z L M N\n"); 251 | printf("%.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n", 252 | t,a1,a2,a3,a4,X,Y,Z,L,M,N); 253 | } 254 | 255 | 256 | 257 | 258 | /* second part of the model (generic): 259 | * simulate a free body in space [Etkin page 104] 260 | * given the applied forces (X,Y,Z) and moments (L,M,N) 261 | * (simple Euler integration) 262 | */ 263 | 264 | /* compute accelerations (body axes) 265 | */ 266 | du = (X / m) - g * sintheta - q * w + r * v; 267 | dv = (Y / m) + g * costheta * sinphi - r * u + p * w; 268 | dw = (Z / m) + g * costheta * cosphi - p * v + q * u; 269 | 270 | /* compute angular accelerations (body axes) 271 | * we must also solve a system of 2 eqns for dp and dr 272 | * we use the solution of 273 | * ax + by = e 274 | * cx + dy = f 275 | * which is (e.g., Cramer's rule) 276 | * x = (de - bf) / (ad - bc) 277 | * y = (af - ce) / (ad - bc) 278 | * to compute dp and dr 279 | */ 280 | a_ = Ix; b_ = -Izx; e_ = L - q * r * (Iz - Iy) + Izx * p * q; 281 | c_ = -Izx; d_ = Iz; f_ = N - p * q * (Iy - Ix) - Izx * q * r; 282 | if (a_*d_ - b_*c_ == 0) { 283 | printf("singularity in (p,q) computation: zero determinant\n"); 284 | exit(0); 285 | } 286 | dp = (d_ * e_ - b_ * f_) / (a_ * d_ - b_ * c_); 287 | dq = (M - r * p * (Ix - Iz) + Izx * (p * p - r * r)) / Iy; 288 | dr = (c_ * e_ - a_ * f_) / (b_ * c_ - a_ * d_); 289 | 290 | /* compute angular velocities (Euler angles) 291 | */ 292 | dphi = p + (q * sinphi + r * cosphi) * tantheta; 293 | dtheta = q * cosphi - r * sinphi; 294 | dpsi = (q * sinphi + r * cosphi) / costheta; 295 | 296 | /* compute velocities (earth axes) 297 | */ 298 | dx = u * costheta * cospsi + 299 | v * (sinphi * sintheta * cospsi - cosphi * sinpsi) + 300 | w * (cosphi * sintheta * cospsi + sinphi * sinpsi); 301 | dy = u * costheta * sinpsi + 302 | v * (sinphi * sintheta * sinpsi + cosphi * cospsi) + 303 | w * (cosphi * sintheta * sinpsi - sinphi * cospsi); 304 | dz = -u * sintheta + 305 | v * sinphi * costheta + 306 | w * cosphi * costheta; 307 | 308 | /* integrate the system of equations 309 | */ 310 | u += du * DT; v += dv * DT; w += dw * DT; 311 | p += dp * DT; q += dq * DT; r += dr * DT; 312 | phi += dphi * DT; theta += dtheta * DT; psi += dpsi * DT; 313 | x += dx * DT; y += dy * DT; z += dz * DT; 314 | 315 | 316 | /* don't go through the ground 317 | */ 318 | if (z > qrstate->z_at_gnd) { 319 | z = qrstate->z_at_gnd; 320 | w = 0; // avoid integration windup 321 | } 322 | 323 | 324 | 325 | 326 | if (qrstate->sim_control.verbose) { 327 | printf("t u v w p q r phi thet psi x y z\n"); 328 | printf("%.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n", 329 | t,u,v,w,p,q,r,phi,theta,psi,x,y,z); 330 | } 331 | if (qrstate->sim_control.pause) { 332 | getchar(); 333 | qrstate->sim_control.pause = ! qrstate->sim_control.pause; 334 | } 335 | 336 | /* update simulation time 337 | */ 338 | t += DT; 339 | 340 | /* update blinking led[0] 341 | */ 342 | if (t - tleds > 0.5) { 343 | leds = (leds ^ 0x0001); 344 | tleds = t; 345 | } 346 | 347 | /* write back to struct 348 | */ 349 | qrstate->t = t; 350 | qrstate->u = u; 351 | qrstate->v = v; 352 | qrstate->w = w; 353 | qrstate->p = p; 354 | qrstate->q = q; 355 | qrstate->r = r; 356 | qrstate->phi = phi; 357 | qrstate->theta = theta; 358 | qrstate->psi = psi; 359 | qrstate->x = x; 360 | qrstate->y = y; 361 | qrstate->z = z; 362 | qrstate->leds = leds; 363 | qrstate->tleds = tleds; 364 | } 365 | 366 | --------------------------------------------------------------------------------