Commit 8797ce8a authored by Ibrahim's avatar Ibrahim
Browse files

Added Demo notebook; 

env.py: Environment clips actions given the action space bounds,
controller.py: renamed from control.py to avoid name conflict with the python control library,
helpers.py: Added more convenience methods to DataLog,
simulation.py: Added conversion/linearization methods to generate a python control library object
parent b1619f33

Demo.ipynb

0 → 100644
+520 −0
Original line number Diff line number Diff line 
%% Cell type:markdown id:d001f534 tags:



#### Notebook setup



%% Cell type:code id:be76bd53 tags:



``` python

%pip install -e .

```



%% Cell type:code id:5a776232 tags:



``` python

%reload_ext autoreload

%autoreload 2

%matplotlib inline

import time

import warnings

import os, sys

from copy import deepcopy

from types import SimpleNamespace



_lib_path, _abs_path = None, os.path.abspath('.')

if os.path.basename(os.path.normpath(_abs_path)) == 'multirotor':

    _lib_path = os.path.abspath(os.path.join(_abs_path, '..'))

elif os.path.isdir('multirotor'):

    _lib_path = _abs_path

if _lib_path is not None and _lib_path not in sys.path:

    sys.path.append(_lib_path)



import matplotlib.pyplot as plt

import gym

import numpy as np

from tqdm.auto import tqdm, trange



from multirotor.helpers import control_allocation_matrix, DataLog

from multirotor.vehicle import MotorParams, VehicleParams, PropellerParams, SimulationParams

from multirotor.controller import PosController, AttController, AltController, Controller

from multirotor.simulation import Multirotor, Propeller, Motor, Battery

from multirotor.visualize import VehicleDrawing

from multirotor.coords import body_to_inertial, inertial_to_body, direction_cosine_matrix, angular_to_euler_rate

from multirotor.env import DynamicsMultirotorEnv as LocalOctorotor

from multirotor.trajectories import Trajectory

```



%% Cell type:code id:421b284b tags:



``` python

# Plotting parameters

SMALL_SIZE = 16

MEDIUM_SIZE = 16

BIGGER_SIZE = 20



plt.rc('font', size=SMALL_SIZE)          # controls default text sizes

plt.rc('axes', titlesize=MEDIUM_SIZE)     # fontsize of the axes title

plt.rc('axes', labelsize=BIGGER_SIZE, titlesize=BIGGER_SIZE)    # fontsize of the x and y labels

plt.rc('xtick', labelsize=MEDIUM_SIZE)    # fontsize of the tick labels

plt.rc('ytick', labelsize=MEDIUM_SIZE)    # fontsize of the tick labels

plt.rc('legend', fontsize=SMALL_SIZE)    # legend fontsize

plt.rc('figure', titlesize=BIGGER_SIZE)  # fontsize of the figure title

```



%% Cell type:markdown id:9a31149f tags:



### Parameters



%% Cell type:code id:1ba2a2fe tags:



``` python

# Tarot T18 params

mp = MotorParams(

    moment_of_inertia=5e-5,

    resistance=0.27,

    k_emf=0.0265

)

pp = PropellerParams(

    moment_of_inertia=1.86e-6,

    use_thrust_constant=True,

    k_thrust=9.8419e-05,

    k_drag=1.8503e-06,

    motor=mp

)

vp = VehicleParams(

    propellers=[pp] * 8,

    angles=np.linspace(0, -2*np.pi, num=8, endpoint=False) + 0.375 * np.pi,

    distances=np.ones(8) * 0.635,

    clockwise=[-1,1,-1,1,-1,1,-1,1],

    mass=10.66,

    inertia_matrix=np.asarray([

        [0.2206, 0, 0],

        [0, 0.2206, 0.],

        [0, 0, 0.4238]

    ])

)

sp = SimulationParams(0.001, 9.81)

```



%% Cell type:markdown id:17f3b34d tags:



### Multirotor



%% Cell type:markdown id:0505e4f6 tags:



#### Motor



%% Cell type:code id:b3128c10 tags:



``` python

%matplotlib inline

plt.figure(figsize=(8,8))

motor = Motor(mp, sp)

for signal in [2, 4, 6, 8, 10, 12, 14, 16, 18, 20]:

    speeds = []

    motor.reset()

    for i in range(200):

        speeds.append(motor.step(signal))

    plt.plot(speeds, label='%dV' % signal)

plt.legend(ncol=2)

plt.ylabel('Speed rad/s')

plt.xlabel('Time /ms')

```



%% Cell type:markdown id:6fb2f5c9 tags:



#### Propeller



%% Cell type:code id:1669687b tags:



``` python

%matplotlib inline

pp_ = deepcopy(pp)

pp_.use_thrust_constant = False

prop = Propeller(pp_, sp)

plt.figure(figsize=(8,8))

speeds = np.linspace(0, 600, num=100)

for a in np.linspace(0, 10, 10, endpoint=False):

    thrusts = []

    for s in speeds:

        thrusts.append(prop.thrust(s, np.asarray([0, 0, a])))

    plt.plot(speeds, thrusts, label='%.1f m/s' % a)

plt.xlabel('Speed rad/s')

plt.ylabel('Thrust /N')

plt.title('Thrust with airspeed')

plt.legend(ncol=2)

```



%% Cell type:markdown id:e7bb489d tags:



#### Multirotor



%% Cell type:code id:a7197b2e tags:



``` python

m = Multirotor(vp, sp)

m.reset()

```



%% Cell type:code id:30aa5565 tags:



``` python

action = m.allocate_control(m.weight, np.asarray([0, 1e-1, 0]))

for i in range(500): m.step_speeds(action)

print(m.orientation * 180 / np.pi)

```



%% Cell type:code id:3216b929 tags:



``` python

m.reset()

ctrl = Controller(

    PosController(0.1, 0.0, 0., 1., dt=1e-3, vehicle=m),

    AttController(np.asarray([10., 10., 0]), 0, 0., 1., dt=1e-3, vehicle=m),

    AltController(10,0,0,1, dt=1e-3, vehicle=m)

)





positions = []

errs = []

alloc_th = []

alloc_to = []

orientations = []

actions = []



targets = [[0,0,2], [0,10,2], [10,10,2], [10,0,2], [0,0,0]]

for i in trange(len(targets), leave=False):

    ref = np.asarray([*targets[i], 0.])

    # Get prescribed dynamics for system

    dynamics = ctrl.step(ref)

    thrust, torques = dynamics[0], dynamics[1:]

    errs.append(ctrl.ctrl_p.err)

    alloc_th.append(thrust)

    alloc_to.append(torques)

    # Convert dynamics into motor RPMs

    action = m.allocate_control(thrust, torques)

    actions.append(action)

    m.step_speeds(action)

    positions.append(m.position)

    orientations.append(m.orientation)

    if i >= 60000: break



positions = np.asarray(positions)

errs = np.asarray(errs)

alloc_th = np.asarray(alloc_th)

alloc_to = np.asarray(alloc_to)

orientations = np.asarray(orientations)

actions = np.asarray(actions)

```



%% Cell type:code id:a045fed5 tags:



``` python

plt.figure(figsize=(14,9))

plt.subplot(1,2,1)

for p,l, c in zip(positions.T, 'xyz', 'rgb'):

    plt.plot(p, label=l, c=c)

lines = plt.gca().lines

plt.twinx()

for p,l,c in zip(orientations.T, ('roll', 'pitch', 'yaw'), 'cmy'):

    plt.plot(p * 180 / np.pi, label=l, ls=':', c=c)

plt.legend(handles=plt.gca().lines+lines)



plt.subplot(1,2,2)

l = plt.plot(alloc_th, label='Ctrl Thrust')

plt.twinx()

for i, c, a in zip(range(3), 'rgb', 'xyz'):

    plt.plot(alloc_to[:,i], label='Ctrl Torque %s' % a, c=c)

plt.legend(handles=plt.gca().lines + l)

```



%% Cell type:markdown id:9eb34672 tags:



### Simulation



%% Cell type:code id:f5f52df4 tags:



``` python

# def wind(t, m):

#     w_inertial = np.asarray([5 * np.sin(t * 2 * np.pi / 4000), 0, 0])

#     dcm = direction_cosine_matrix(*m.orientation)

#     return inertial_to_body(w_inertial, dcm)[:2]

wind = lambda t, m: [0, 0]

```



%% Cell type:code id:a98de44f tags:



``` python

m = Multirotor(vp, sp)

m.reset()

env = LocalOctorotor(vehicle=m)

env.vehicle.state[2] = 1.

traj = Trajectory([[0,0,2], [15,0,2], [15,15,2], [0,15,2], [0,0,2]],

                  env.vehicle, proximity=2, resolution=None)



for _ in range(5000):

    state, *_ = env.step(np.asarray([0., 0., vp.mass * sp.g * 1.01, 0., 0., 0.]))

```



%% Cell type:code id:dea77bcb tags:



``` python

ctrl = Controller(

    # (Outer) Position controller has low sensitivity

    PosController(0.5, 0.02, 0., 1., dt=1e-3, vehicle=m),

    # (Inner) Attitude controller is more responsive (except for yaw, which we are not controlling)

    AttController(np.asarray([50., 50., 2.]),

                  np.asarray([10., 10., 2.]),

                  np.asarray([0., 0., 0.]), 1., dt=1e-3, vehicle=m),

    # Altitude controller is more responsive as well

    AltController(50, 2, 0, 1, dt=1e-3, vehicle=m)

)



errs = SimpleNamespace()

errs.pos = SimpleNamespace()

errs.pos.p, errs.pos.i, errs.pos.d = [], [], []

errs.att = SimpleNamespace()

errs.att.p, errs.att.i, errs.att.d = [], [], []



log = DataLog(env.vehicle, ctrl, 'action', 'target', 'alloc_errs')



for i, pos in tqdm(enumerate(traj), leave=False, total=60000):

    if i==60000: break

    # Generate reference for controller

    ref = np.asarray([*pos, 0.])

    # Get prescribed dynamics for system as thrust and torques

    dynamics = ctrl.step(ref)

    thrust, torques = dynamics[0], dynamics[1:]

    # Allocate control: Convert dynamics into motor rad/s

    action = m.allocate_control(thrust, torques)

    # Add any faults/restrictions to motor dynamics

    action = np.clip(action, a_min=0, a_max=400)

    # if i > 30000: action[3] = min(action[3], 300)

    # Get resultant forces/torques given propeller states

    f, t = m.get_forces_torques(action, state)

    # Add environmental disturbances

    f[:2] += wind(i, env.vehicle)

    # Send dynamics to environment

    state, *_ = env.step(np.asarray([*f, *t]))



    for (c, e) in zip((ctrl.ctrl_p, ctrl.ctrl_a), (errs.pos, errs.att)):

        e.p.append(c.err_p)

        e.i.append(c.err_i)

        e.d.append(c.err_d)

    alloc_errs = np.asarray([thrust, *torques]) - m.alloc @ action**2



    log.log(action=action, target=pos, alloc_errs=alloc_errs)



    if np.any(np.abs(m.orientation[:2]) > np.pi/6): break



log.done_logging()

for e in (errs.pos, errs.att):

    e.p = np.asarray(e.p)

    e.i = np.asarray(e.i)

    e.d = np.asarray(e.d)

```



%% Cell type:code id:81a8043e tags:



``` python

%matplotlib inline

plt.figure(figsize=(21,10.5))

plot_grid = (3,3)

plt.subplot(*plot_grid,1)



n = len(log)



plt.plot(log.x, label='x', c='r')

plt.plot(log.target[:, 0], c='r', ls=':')

plt.plot(log.y, label='y', c='g')

plt.plot(log.target[:, 1], c='g', ls=':')

plt.plot(log.z, label='z', c='b')

lines = plt.gca().lines[::2]

plt.ylabel('Position /m')

plt.twinx()

plt.plot(log.roll * (180 / np.pi), label='roll', c='c', ls=':')

plt.plot(log.pitch * (180 / np.pi), label='pitch', c='m', ls=':')

plt.plot(log.yaw * (180 / np.pi), label='yaw', c='y', ls=':')

plt.ylabel('Orientation /deg')

plt.legend(handles=plt.gca().lines + lines, ncol=2)

plt.title('Position and Orientation')



plt.subplot(*plot_grid,2)

for i in range(log.action.shape[1]):

    l, = plt.plot(log.action[:,i], label='prop %d' % i)

#     plt.plot(speeds[:,i], c=l.get_c())

lines = plt.gca().lines

plt.legend(handles=lines, ncol=2)

plt.title('Motor speeds /RPM')





plt.subplot(*plot_grid,3)

v_world = np.zeros_like(log.velocity)

for i, (v, o) in enumerate(zip(log.velocity, log.orientation)):

    dcm = direction_cosine_matrix(*o)

    v_world[i] = body_to_inertial(v, dcm)

for i, c, a in zip(range(3), 'rgb', 'xyz'):

    plt.plot(v_world[:,i], label='Velocity %s' % a, c=c)

#     plt.plot(velocities[:,i], label='Velocity %s' % a, c=c)

plt.legend()

plt.title('Velocities')



plt.subplot(*plot_grid,4)

plt.title('Controller allocated dynamics')

l = plt.plot(log.thrust, label='Ctrl Thrust')

plt.ylabel('Force /N')

plt.twinx()

for i, c, a in zip(range(3), 'rgb', 'xyz'):

    plt.plot(log.torques[:,i], label='Ctrl Torque %s' % a, c=c)

plt.ylabel('Torque /Nm')

plt.legend(handles=plt.gca().lines + l, ncol=2)



plt.subplot(*plot_grid,5)

lines = plt.plot(log.alloc_errs[:, 0], label='Thrust err', c='b')

plt.ylabel('Thrust /N')

plt.twinx()

plt.plot(log.alloc_errs[:, 1], label='Torque x err', ls=':')

plt.plot(log.alloc_errs[:, 2], label='Torque y err', ls=':')

plt.plot(log.alloc_errs[:, 3], label='Torque z err', ls=':')

plt.legend(handles = plt.gca().lines + lines, ncol=2)

plt.ylabel('Torque /Nm')

plt.title('Allocation Errors')



plt.subplot(*plot_grid,6)

plt.plot(log.target[:,0], log.target[:,1], label='Prescribed traj')

plt.plot(log.x, log.y, label='Actual traj')

plt.gca().set_aspect('equal', 'box')

plt.title('XY positions /m')

plt.legend()



plt.tight_layout()

```



%% Cell type:code id:c17d8b15 tags:



``` python

%matplotlib inline

lines = []

plt.figure(figsize=(21,24))

plt.subplot(6,1,1)

plt.plot(errs.pos.p[:,0], label='Pos-x P', c='r', ls='-')

plt.plot(errs.pos.p[:,1], label='Pos-y P', c='g', ls='-')

lines += plt.gca().lines

plt.title('Position P errors')

plt.subplot(6,1, 2)

plt.plot(errs.pos.i[:, 0], label='Pos-x I', c='r', ls=':')

plt.plot(errs.pos.i[:, 1], label='Pos-y I', c='g', ls=':')

lines += plt.gca().lines

plt.title('Position I errors')

plt.subplot(6,1, 3)

plt.plot(errs.pos.d[:, 0], label='Pos-x D', c='r', ls='-.')

plt.plot(errs.pos.d[:, 1], label='Pos-y D', c='g', ls='-.')

lines += plt.gca().lines

plt.legend(handles=lines)

plt.title('Position D errors')

lines = []

plt.subplot(6,1,4)

plt.plot(errs.att.p[:,0], label='Att-x P', c='r', ls='-')

plt.plot(errs.att.p[:,1], label='Att-y P', c='g', ls='-')

lines += plt.gca().lines

plt.title('Position P errors')

plt.subplot(6,1, 5)

plt.plot(errs.att.i[:, 0], label='Att-x I', c='r', ls=':')

plt.plot(errs.att.i[:, 1], label='Att-y I', c='g', ls=':')

lines += plt.gca().lines

plt.title('Position I errors')

plt.subplot(6,1, 6)

plt.plot(errs.att.d[:, 0], label='Att-x D', c='r', ls='-.')

plt.plot(errs.att.d[:, 1], label='Att-y D', c='g', ls='-.')

lines += plt.gca().lines

plt.legend(handles=lines)

plt.title('Position D errors')

```



%% Cell type:code id:d9d99572 tags:



``` python

%matplotlib notebook

fig = plt.figure()

xlim = ylim = zlim = (np.min(log.position), np.max(log.position))

ax = fig.add_subplot(projection='3d', xlim=xlim, ylim=ylim, zlim=zlim)

ax.set_xlabel('x')

ax.set_ylabel('y')

ax.set_zlabel('z')

ax.plot(log.x, log.y, log.z)

```



%% Cell type:markdown id:e87063b0 tags:



### Variations



%% Cell type:code id:7c0024dc tags:



``` python

from multirotor.helpers import vehicle_params_factory

```



%% Cell type:code id:4a591d5b tags:



``` python

vp4 = vehicle_params_factory(4, m_prop=0.125, d_prop=0.25, params=pp, m_body=4)

vp6 = vehicle_params_factory(6, m_prop=0.125, d_prop=0.5, params=pp, m_body=6)

vp8 = vehicle_params_factory(8, m_prop=0.125, d_prop=0.75, params=pp, m_body=8)

wind = lambda t, m: [2, 0]

logs = []

for vpx in tqdm([vp4, vp6, vp8], leave=False):

    m = Multirotor(vpx, sp)

    env = LocalOctorotor(vehicle=m, allocate=True, max_rads=400)

    env.reset()

    traj = Trajectory([[2.5,5,0, 2000], [5,0,0,2000], [0,0,0,2000]],

                      env.vehicle, proximity=0.2, resolution=None)



    ctrl = Controller(

        # (Outer) Position controller has low sensitivity

        PosController(0.2, 0.02, 0., 1., dt=sp.dt, vehicle=m, max_tilt=np.pi/18),

        # (Inner) Attitude controller is more responsive (except for yaw, which we are not controlling)

        AttController(np.asarray([50., 50., 0.]),

                      np.asarray([50., 50., 0.]),

                      np.asarray([1., 1., 0.]), 1., dt=sp.dt, vehicle=env.vehicle),

        # Altitude controller is more responsive as well

        AltController(50, 2, 0, 1, dt=1e-3, vehicle=m)

    )



    log = DataLog(env.vehicle, ctrl, 'targets')



    for i, pos in tqdm(enumerate(traj), leave=False, total=60000):

        if i==60000: break

        # Get prescribed dynamics for system

        reference = np.asarray([*pos, 0.])

        dynamics = ctrl.step(reference)

        thrust, torques = dynamics[0], dynamics[1:]

        forces = np.asarray([0, 0, thrust])

        # Add disturbances

        f[:2] += wind(i, env.vehicle)

        # apply to simulation

        state, *_ = env.step(np.asarray([*forces, *torques]))



        log.log(targets=pos)



    log.done_logging()

    logs.append(log)

```



%% Cell type:code id:0b8e3a15 tags:



``` python

%matplotlib inline

plt.figure(figsize=(6,6))

for log, label, ls, lw in zip(

    logs,

    ('quad-rotor', 'hexa-rotor', 'octo-rotor'),

    ('-','--',':'),

    (2,3,4)

):

    plt.plot(log.x, log.y, label=label, ls=ls, lw=lw)

plt.gca().set_aspect('equal', 'box')

plt.title('XY positions /m')

plt.grid(which='both')

# plt.xlim(0, 5)

# plt.ylim(0, 5)

plt.legend()

```

control.py→controller.py

+0 −0

File moved.

View file

env.py

+9 −4
Original line number Diff line number Diff line
@@ -6,6 +6,7 @@ import numpy as np 
import gym



from .simulation import Multirotor

from .helpers import find_nominal_speed





class BaseMultirotorEnv(gym.Env):
@@ -41,7 +42,7 @@ class BaseMultirotorEnv(gym.Env): 
class DynamicsMultirotorEnv(BaseMultirotorEnv):





    def __init__(self, vehicle: Multirotor=None, allocate: bool=False) -> None:

    def __init__(self, vehicle: Multirotor=None, allocate: bool=False, max_rads=None) -> None:

        super().__init__(vehicle=vehicle)

        

        self.action_space = gym.spaces.Box(
@@ -51,13 +52,15 @@ class DynamicsMultirotorEnv(BaseMultirotorEnv): 
            shape=(6,)  # 3 forces, 3 torques

        )

        self.allocate = allocate

        self.max_rads = max_rads





    def step(self, action: np.ndarray):

        if self.allocate:

            speeds = self.vehicle.allocate_control(action[0], action[3:6])

            speeds = self.vehicle.allocate_control(action[2], action[3:6])

            speeds = np.clip(speeds, a_min=0, a_max=self.max_rads)

            forces, torques = self.vehicle.get_forces_torques(speeds, self.vehicle.state)

            action = np.concatenate(forces, torques)

            action = np.concatenate((forces, torques))

        self.state = self.vehicle.step_dynamics(u=action)

        return self.state, None, None, None
@@ -66,7 +69,7 @@ class DynamicsMultirotorEnv(BaseMultirotorEnv): 
class SpeedsMultirotorEnv(gym.Env):





    def __init__(self, vehicle: Multirotor=None) -> None:

    def __init__(self, vehicle: Multirotor=None, max_rads=None) -> None:

        super().__init__(vehicle=vehicle)

        

        self.action_space = gym.spaces.Box(
@@ -75,8 +78,10 @@ class SpeedsMultirotorEnv(gym.Env): 
            dtype=np.float32,

            shape=(len(vehicle.propellers),)  # action for each propeller

        )

        self.max_rads = max_rads





    def step(self, action: np.ndarray):

        action = np.clip(action, a_min=0, a_max=self.max_rads)

        self.state = self.vehicle.step_speeds(u=action)

        return self.state, None, None, None
 
 No newline at end of file

helpers.py

+16 −0
Original line number Diff line number Diff line
@@ -316,6 +316,13 @@ class DataLog: 
        self._arrayed = True





    def __len__(self):

        if len(self._states)==0:

            if self.states is not None:

                return len(self.states)

        return len(self._states)



        

    @property

    def position(self):

        self._make_arrays()
@@ -338,6 +345,15 @@ class DataLog: 
        self._make_arrays()

        return self.states[:, 6:9]

    @property

    def roll(self):

        return self.orientation[:, 0].reshape(-1)

    @property

    def pitch(self):

        return self.orientation[:, 1].reshape(-1)

    @property

    def yaw(self):

        return self.orientation[:, 2].reshape(-1)

    @property

    def angular_rate(self):

        self._make_arrays()

        return self.states[:, 9:12]

physics.py

+0 −2
Original line number Diff line number Diff line
@@ -4,8 +4,6 @@ import numpy as np 
from numba import njit

from scipy.optimize import fsolve



from .coords import body_to_inertial, direction_cosine_matrix







@njit