controller.py: controller objects have state (c63c6bc8) · Commits · Ibrahim Ahmed / multirotor

Demo.ipynb

+37 −69

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)
		# _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, -2np.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
		#### Vehicle

		%% Cell type:code id:a7197b2e tags:
		%% Cell type:code id:ecc23e14 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:
		%% Cell type:markdown id:2c6535f4 tags:

		### PID Controller

		%% Cell type:code id:a7197b2e tags:

		``` python
		m.reset()
		m = Multirotor(vp, sp)
		pos = PosController(0.1, 0.0, 0., 1., dt=1e-3, vehicle=m)
		att = AttController(np.asarray([10., 10., 0]), 0, 0., 1., dt=1e-3, vehicle=m)
		alt = AltController(10,0,0,1, dt=1e-3, vehicle=m)
		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)
		pos, att, alt
		)
		```

		%% Cell type:code id:d21d73fe tags:

		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)
		``` python
		log = DataLog(controller=ctrl)
		for i in range(100):
		action = ctrl.step((1,1,1,0))
		log.log()
		log.done_logging()
		```

		%% Cell type:code id:151eba6a tags:

		plt.subplot(1,2,2)
		l = plt.plot(alloc_th, label='Ctrl Thrust')
		``` python
		plt.plot(log.actions[:,0], ls=':', label='thrust')
		lines = plt.gca().lines
		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)
		for s, axis in zip(log.actions.T[1:], ('x','y','z')):
		plt.plot(s, label=axis + '-torque')
		plt.legend(handles=plt.gca().lines + lines)
		```

		%% 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')
		log = DataLog(env.vehicle, ctrl, other_vars=('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()
		```

multirotor/controller.py

+29 −12

Original line number	Diff line number	Diff line
		@@ -33,21 +33,30 @@ class PIDController:
		"Simulation parameters to set timestep"

		def __post_init__(self):
		self.action = None
		self.err_p = np.zeros_like(self.k_p)
		self.err_i = np.zeros_like(self.k_i)
		self.err_d = np.zeros_like(self.k_d)
		self.err = np.zeros_like(self.k_p)
		self.state = None
		if self.max_err_i is None:
		self.max_err_i = np.inf


		def reset(self):
		self.action = None
		self.err *= 0
		self.err_p *= 0
		self.err_i *= 0
		self.err_d *= 0
		self.state = None


		@property
		def state(self) -> np.ndarray:
		return np.concatenate((
		np.atleast_1d(self.err_p),
		np.atleast_1d(self.err_i),
		np.atleast_1d(self.err_d)
		))


		def step(self, reference: np.ndarray, measurement: np.ndarray) -> np.ndarray:
		@@ -75,8 +84,8 @@ class PIDController:
		)
		self.err_d = (err - self.err) / self.dt
		self.err = err
		self.state = self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d
		return self.state
		self.action = self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d
		return self.action



		@@ -131,7 +140,7 @@ class PosController(PIDController):
		# ctrl[1] -> y dir -> roll -> lateral
		ctrl[0:2] = np.clip(ctrl[0:2], a_min=-self.max_tilt, a_max=self.max_tilt)
		ctrl[1] *= -1 # +y motion requires negative roll
		self.state = ctrl
		self.action = ctrl
		return ctrl # desired pitch, roll


		@@ -165,8 +174,8 @@ class AttController(PIDController):
		# prescribed change in velocity i.e. angular acceleration
		ctrl = super().step(reference=ref_delta, measurement=mea_delta)
		# torque = moment of inertia . angular_acceleration
		self.state = self.vehicle.params.inertia_matrix.dot(ctrl)
		return self.state
		self.action = self.vehicle.params.inertia_matrix.dot(ctrl)
		return self.action



		@@ -182,6 +191,7 @@ class AltController(PIDController):

		vehicle: Multirotor


		def step(self, reference, measurement):
		roll, pitch, yaw = self.vehicle.orientation
		# desired change in z i.e. velocity
		@@ -195,7 +205,7 @@ class AltController(PIDController):
		ctrl / (np.cos(roll) * np.cos(pitch))
		) + \
		self.vehicle.weight
		self.state = ctrl
		self.action = ctrl
		return ctrl # thrust force


		@@ -213,24 +223,31 @@ class Controller:
		self.ctrl_p = ctrl_p
		self.ctrl_a = ctrl_a
		self.ctrl_z = ctrl_z
		self.state = np.zeros(4)
		self.vehicle = self.ctrl_a.vehicle
		self.action = None
		assert self.ctrl_a.vehicle is self.ctrl_p.vehicle, "Vehicle instances different."
		assert self.ctrl_a.vehicle is self.ctrl_z.vehicle, "Vehicle instances different."


		def reset(self):
		self.state = np.zeros(4)
		self.action = None
		self.ctrl_a.reset()
		self.ctrl_p.reset()
		self.ctrl_z.reset()


		@property
		def state(self) -> np.ndarray:
		return np.concatenate(
		(self.ctrl_p.state, self.ctrl_a.state, self.ctrl_z.state)
		)


		def step(self, reference, measurement=None):
		# x,y,z,yaw
		pitch_roll = self.ctrl_p.step(reference[:2], self.vehicle.position[:2])
		ref_orientation = np.asarray([pitch_roll[1], pitch_roll[0], reference[3]])
		torques = self.ctrl_a.step(ref_orientation, self.vehicle.orientation)
		thrust = self.ctrl_z.step(reference[2], self.vehicle.position[2])
		self.state = np.asarray([thrust, *torques])
		return self.state
		self.action = np.asarray([thrust, *torques])
		return self.action

multirotor/helpers.py

+8 −7

Original line number	Diff line number	Diff line
		@@ -229,7 +229,7 @@ class DataLog:
		"""
		def __init__(
		self, vehicle: 'Multirotor'=None, controller: 'Controller'=None,
		*other_vars
		other_vars=None
		):
		"""
		Parameters
		@@ -239,15 +239,15 @@ class DataLog:
		controller : Controller, optional
		The controller instance to track, by default None
		"""
		return self.track(vehicle, controller, *other_vars)
		return self.track(vehicle, controller, other_vars)


		def track(self, vehicle, controller, *other_vars):
		def track(self, vehicle, controller, other_vars=None):
		"""
		Register Multirotor and Controller instances to track, along with names
		of any other variables to be manually added.

		>>> DataLog.track(Multirotor(), Controller(), 'error')
		>>> DataLog.track(Multirotor(), Controller(), other_vars=('error',))

		Parameters
		----------
		@@ -266,7 +266,7 @@ class DataLog:
		self.states = None
		self._actions = []
		self.actions = None
		self._args = other_vars
		self._args = () if other_vars is None else other_vars
		for arg in self._args:
		setattr(self, arg, None)
		setattr(self, '_' + str(arg), [])
		@@ -286,7 +286,7 @@ class DataLog:
		if self.vehicle is not None:
		self._states.append(self.vehicle.state)
		if self.controller is not None:
		self._actions.append(self.controller.state)
		self._actions.append(self.controller.action)
		for key, value in kwargs.items():
		getattr(self, '_' + key).append(value)

		@@ -365,3 +365,4 @@ class DataLog:
		def torques(self):
		self._make_arrays()
		return self.actions[:, 1:4]
		# TODO: add properties for controller state

multirotor/simulation.py

+20 −13

Original line number	Diff line number	Diff line
		@@ -225,6 +225,8 @@ class Multirotor:
		----------
		params : VehicleParams
		The vehicle parameters. These completely describe the vehicle's properties.
		The parameters are copied by this class, so any changes made to the params
		object is isolated from this instance.
		simulation : SimulationParams
		The simulation parameters.
		"""
		@@ -238,6 +240,8 @@ class Multirotor:
		self.propellers = []
		for params in self.params.propellers:
		self.propellers.append(Propeller(params, self.simulation))
		self.dtype = self.params.inertia_matrix.dtype if simulation.dtype is None \
		else simulation.dtype
		self.reset()


		@@ -259,12 +263,13 @@ class Multirotor:
		x = cos(self.params.angles) * self.params.distances
		y = sin(self.params.angles) * self.params.distances
		z = np.zeros_like(y)
		self.propeller_vectors = np.vstack((x, y, z))
		self.propeller_vectors = np.vstack((x, y, z)).astype(self.dtype)

		self.inertial_matrix_inverse = np.asmatrix(np.linalg.inv(self.params.inertia_matrix))
		self.alloc, self.alloc_inverse = control_allocation_matrix(self.params)

		self.state = np.zeros(12)
		self.alloc = self.alloc.astype(self.dtype)
		self.alloc_inverse = self.alloc_inverse.astype(self.dtype)
		self.params.inertia_matrix_inverse = self.params.inertia_matrix_inverse.astype(self.dtype)
		self.state = np.zeros(12, dtype=self.dtype)
		return self.state


		@@ -335,7 +340,7 @@ class Multirotor:
		linear_vel_body,
		angular_vel_body)

		thrust_vec = np.zeros((3, len(self.propellers)))
		thrust_vec = np.zeros((3, len(self.propellers)), dtype=self.dtype)
		torque_vec = np.zeros_like(thrust_vec)

		for i, (speed, prop, clockwise) in enumerate(zip(
		@@ -413,7 +418,8 @@ class Multirotor:
		# This method must not have any side-effects. It should not change the
		# state of the vehicle. This method is called multiple times from the
		# same state by the odeint() function, and the results should be consistent.
		forces, torques = self.get_forces_torques(u, x)
		forces, torques = self.get_forces_torques(
		u, x)
		xdot = apply_forces_torques(
		forces, torques, x, self.simulation.g,
		self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse)
		@@ -438,7 +444,8 @@ class Multirotor:
		"""
		self.t += self.simulation.dt
		self.state = odeint(
		self.dxdt_dynamics, self.state, (0, self.simulation.dt), args=(u,),
		self.dxdt_dynamics, self.state, (0, self.simulation.dt),
		args=(u,),
		rtol=1e-4, atol=1e-4, tfirst=True
		)[-1]
		self.state = np.around(self.state, 4)
		@@ -533,9 +540,9 @@ class Multirotor:
		'xrate', 'yrate', 'zrate']
		)
		if linearize:
		x0 = np.zeros(12) if about_state==0 else about_state
		u0 = np.zeros(6) if about_action==0 else about_action
		sys = sys.linearize(eps=perturbation, x0=x0, u0=x0)
		x0 = (np.zeros(12) if about_state==0 else about_state)
		u0 = (np.zeros(6) if about_action==0 else about_action)
		sys = sys.linearize(eps=perturbation, x0=x0, u0=u0)
		return sys


		@@ -573,7 +580,7 @@ class Multirotor:
		'xrate', 'yrate', 'zrate']
		)
		if linearize:
		x0 = np.zeros(12) if about_state==0 else about_state
		u0 = np.zeros(len(self.propellers)) if about_action==0 else about_action
		sys = sys.linearize(eps=perturbation, x0=x0, u0=x0)
		x0 = (np.zeros(12) if about_state==0 else about_state)
		u0 = (np.zeros(len(self.propellers)) if about_action==0 else about_action)
		sys = sys.linearize(eps=perturbation, x0=x0, u0=u0)
		return sys

multirotor/vehicle.py

+2 −0

Original line number	Diff line number	Diff line
		@@ -121,6 +121,8 @@ class SimulationParams:
		"""Gravitational acceleration"""
		rho: float = 1.225
		"Air density kg/m^3 at MSL"
		dtype: type = None
		"Default data type for arrays. If None, inferred from VehicleParams inertia_matrix."

Admin message