Helper functions to controller. Visualization improvements (57473f84) · Commits · Ibrahim Ahmed / multirotor

multirotor/controller/pid.py

+84 −13

Changes for multirotor/controller/pid.py: 84 added lines, 13 removed lines.
Original line number	Diff line number	Diff line
		from dataclasses import dataclass
		from typing import Union, Dict

		import numpy as np
		from scipy.integrate import trapezoid
		@@ -76,6 +77,7 @@ class PIDController:
		else:
		self.max_err_i = np.asarray(self.max_err_i, dtype=self.err.dtype)
		self.action = np.zeros_like(self.err)
		self._params = ('k_p', 'k_i', 'k_d', 'max_err_i')


		def reset(self):
		@@ -86,6 +88,29 @@ class PIDController:
		self.err_d *= 0


		def set_params(self, **params: Dict[str, Union[np.ndarray, bool, float, int]]):
		for name, param in params.items():
		if hasattr(self, name):
		attr = getattr(self, name)
		if isinstance(attr, np.ndarray):
		param = np.asarray(param, dtype=attr.dtype)
		# cast to an axis & assign in-place
		# for cases where float param is assigned to array attr
		if attr.ndim > 0:
		attr[:] = param
		else:
		attr = param
		else:
		attr = param
		setattr(self, name, attr)
		else:
		raise AttributeError('Attribute %s not part of class.' % name)


		def get_params(self) -> Dict[str, np.ndarray]:
		return {name: getattr(self, name) for name in self._params}


		@property
		def state(self) -> np.ndarray:
		return np.concatenate((
		@@ -120,6 +145,7 @@ class PIDController:
		if ref_is_error:
		err = reference
		else:
		self.reference = reference
		err = reference - measurement
		self.err_p = self.k_p * err
		self.err_i = self.k_i * np.clip(
		@@ -159,6 +185,8 @@ class PosController(PIDController):
		if self.leashing or self.square_root_scaling:
		self.k_p[:] = 0.5 * self.max_jerk / self.max_acceleration
		super().__post_init__()
		self._params = tuple(list(self._params) + \
		['max_velocity', 'max_acceleration', 'max_jerk', 'square_root_scaling', 'leashing'])


		@property
		@@ -180,6 +208,7 @@ class PosController(PIDController):

		def step(self, reference, measurement, dt):
		# inertial frame velocity
		self.reference = reference
		err = reference - measurement
		err_len = np.linalg.norm(err)
		# TODO check conditional logic
		@@ -234,6 +263,7 @@ class VelController(PIDController):
		self.k_i = np.ones(2) * np.asarray(self.k_i)
		self.k_d = np.ones(2) * np.asarray(self.k_d)
		super().__post_init__()
		self._params = tuple(list(self._params) + ['max_tilt'])


		def step(self, reference, measurement, dt):
		@@ -271,10 +301,13 @@ class AttController(PIDController):
		if self.square_root_scaling:
		self.k_p[:] = self.max_jerk / self.max_acceleration
		super().__post_init__()
		self._params = tuple(list(self._params) + \
		['max_acceleration', 'max_jerk', 'square_root_scaling'])


		def step(self, reference, measurement, dt):
		err = reference - measurement
		self.reference = reference
		err_len = np.linalg.norm(err)
		if self.square_root_scaling and err_len > 0:
		velocity = np.zeros_like(self.k_p)
		@@ -313,6 +346,7 @@ class RateController(PIDController):
		self.k_i = np.ones(3) * np.asarray(self.k_i)
		self.k_d = np.ones(3) * np.asarray(self.k_d)
		super().__post_init__()
		self._params = tuple(list(self._params) + ['max_acceleration'])


		def step(self, reference, measurement, dt):
		@@ -353,6 +387,7 @@ class AltController(PIDController):
		self.k_i = np.zeros(1) * np.asarray(self.k_i)
		self.k_d = np.zeros(1) * np.asarray(self.k_d)
		super().__post_init__()
		self._params = tuple(list(self._params) + ['max_velocity'])


		def step(
		@@ -401,9 +436,10 @@ class Controller:
		ctrl_p: PosController, ctrl_v: VelController,
		ctrl_a: AttController, ctrl_r: RateController,
		ctrl_z: AltController, ctrl_vz: AltRateController,
		interval_p: float=1.,
		interval_a: float=1.,
		interval_z: float=1.
		period_p: float=1.,
		period_a: float=1.,
		period_z: float=1.,
		feedforward_weight: float=0.
		):
		"""
		Parameters
		@@ -420,7 +456,7 @@ class Controller:
		The altitude controller
		ctrl_vz : AltRateController
		The altitude rate controller
		interval_[p \| a \| z] : float, optional
		period_[p \| a \| z] : float, optional
		The time resolution of the [position \| attitude \| altitude] controller.
		The controller will only renew an action after this interval has passed.
		Otherwise it will apply the last action. For e.g. if interval=1, and vehicle dt=0.1, a
		@@ -433,12 +469,13 @@ class Controller:
		self.ctrl_z = ctrl_z
		self.ctrl_vz = ctrl_vz
		self.vehicle = self.ctrl_a.vehicle
		self.interval_p = interval_p
		self.interval_a = interval_a
		self.interval_z = interval_z
		self.steps_p = int(self.interval_p // self.vehicle.simulation.dt)
		self.steps_a = int(self.interval_a // self.vehicle.simulation.dt)
		self.steps_z = int(self.interval_z // self.vehicle.simulation.dt)
		self.period_p = period_p
		self.period_a = period_a
		self.period_z = period_z
		self.steps_p = int(self.period_p // self.vehicle.simulation.dt)
		self.steps_a = int(self.period_a // self.vehicle.simulation.dt)
		self.steps_z = int(self.period_z // self.vehicle.simulation.dt)
		self.feedforward_weight = feedforward_weight
		assert self.ctrl_a.vehicle is self.ctrl_p.vehicle, "Vehicle instances different."
		assert self.ctrl_a.vehicle is self.ctrl_v.vehicle, "Vehicle instances different."
		assert self.ctrl_a.vehicle is self.ctrl_r.vehicle, "Vehicle instances different."
		@@ -449,9 +486,12 @@ class Controller:

		def reset(self):
		self.action = np.zeros(4, self.vehicle.dtype)
		self.reference = np.zeros_like(self.action)
		self.thrust = None
		self.torques = None
		self._ref_vel = np.zeros(2, self.vehicle.dtype)
		self._pid_vel = np.zeros_like(self._ref_vel)
		self._scurve_vel = np.zeros_like(self._ref_vel)
		self._pitch_roll = np.zeros(2, self.vehicle.dtype)
		self.n = 0
		self.t = self.vehicle.t
		@@ -464,10 +504,40 @@ class Controller:
		return self.state


		def set_params(self, **params):
		self.ctrl_p.set_params(**params.get('ctrl_p', {}))
		self.ctrl_v.set_params(**params.get('ctrl_v', {}))
		self.ctrl_a.set_params(**params.get('ctrl_a', {}))
		self.ctrl_r.set_params(**params.get('ctrl_r', {}))
		self.ctrl_a.set_params(**params.get('ctrl_a', {}))
		self.ctrl_vz.set_params(**params.get('ctrl_vz', {}))
		local_params = {k:v for k,v in params.items() if not isinstance(v, dict)}
		for name, param in local_params.items():
		if hasattr(self, name):
		# Controller class has no np.array attributes, so no casting/
		# in-place assignment needed like tith PIDController class's
		# set_params() method
		setattr(self, name, param)


		def get_params(self) -> Dict[str, Dict[str, np.ndarray]]:
		p = dict(
		ctrl_p=self.ctrl_p.get_params(),
		ctrl_v=self.ctrl_v.get_params(),
		ctrl_a=self.ctrl_a.get_params(),
		ctrl_r=self.ctrl_r.get_params(),
		ctrl_z=self.ctrl_z.get_params(),
		ctrl_vz=self.ctrl_vz.get_params(),
		feedforward_weight=self.feedforward_weight
		)
		return p


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


		@@ -481,6 +551,7 @@ class Controller:
		ref_z = self.vehicle.position[2] + error[2]
		ref_yaw = self.vehicle.orientation[2] + error[3]
		else:
		self.reference = reference
		ref_xy = reference[:2]
		ref_z = reference[2]
		ref_yaw = reference[3]
		@@ -492,9 +563,9 @@ class Controller:

		if self.n % self.steps_p == 0:
		dt = self.steps_p * self.vehicle.simulation.dt
		self._ref_vel = self.ctrl_p.step(ref_xy, self.vehicle.position[:2], dt=dt)
		self._pid_vel = self._ref_vel = self.ctrl_p.step(ref_xy, self.vehicle.position[:2], dt=dt)
		if feed_forward_velocity is not None:
		self._ref_vel += feed_forward_velocity
		self._ref_vel = (self.feedforward_weight * feed_forward_velocity[:2]) + (1 - self.feedforward_weight) * self._pid_vel
		self._ref_vel = np.clip(self._ref_vel, -self.ctrl_p.max_velocity, self.ctrl_p.max_velocity)

		if self.n % self.steps_a == 0:

multirotor/helpers.py

+121 −7

Changes for multirotor/helpers.py: 121 added lines, 7 removed lines.
Original line number	Diff line number	Diff line
		from typing import Callable, Iterable, Tuple
		from types import SimpleNamespace

		import numpy as np
		from scipy.optimize import fsolve
		from tqdm.autonotebook import tqdm

		from .vehicle import PropellerParams, VehicleParams
		from .vehicle import PropellerParams, VehicleParams, SimulationParams
		from .physics import torque



		@@ -228,6 +229,55 @@ def control_allocation_matrix(params: VehicleParams) -> Tuple[np.ndarray, np.nda



		def get_vehicle_ability(
		vp: VehicleParams, sp: SimulationParams,
		max_tilt: float=np.pi/12,
		max_angular_acc: float=5,
		max_rads: float=600
		):
		alloc, alloc_inverse = control_allocation_matrix(vp)
		I_roll, I_pitch, I_yaw = vp.inertia_matrix.diagonal()
		n = len(vp.propellers)

		thrusts = [p.k_thrust * max_rads**2 for p in vp.propellers]
		max_f = sum(thrusts)
		max_acc_z = (max_f - (vp.mass * sp.g)) / vp.mass

		# Thrust produced at max_tilt to keep vehicle altitude constant.
		# Use that to calculate lateral component of thrust and lateral acceleration
		tilt_hover_thrust = (vp.mass * sp.g) / (n * np.cos(max_tilt))
		# Thrust is limited by what is possible by the propellers
		tilt_hover_thrust = min(max_f, tilt_hover_thrust)
		max_acc_xy = n * tilt_hover_thrust * np.sin(max_tilt) / vp.mass

		thrust_vec = np.zeros((3, n))
		# excessive thrust (above weight) that can contribute to torque
		thrust_vec[2] = np.asarray(thrusts) - (vp.mass * sp.g / n)
		k_drag_vec = np.asarray([p.k_drag for p in vp.propellers])
		inertia_vec = np.asarray([p.moment_of_inertia for p in vp.propellers])
		torques = torque(
		position_vector=vp.propeller_vectors,
		force=thrust_vec,
		clockwise=np.asarray(vp.clockwise).astype(float),
		drag_coefficient=k_drag_vec,
		moment_of_inertia=inertia_vec,
		prop_angular_acceleration=0,
		prop_angular_velocity=max_rads
		)
		torques = (torques * (torques > 0)).sum(axis=1)
		I = vp.inertia_matrix.diagonal()
		# t = i a
		ang_acc = torques / I

		res = dict(
		max_acc_xy=max_acc_xy,
		max_acc_z=max_acc_z,
		max_ang_acc=max(ang_acc)
		)
		return res



		class DataLog:
		"""
		Records state and action variables for a multirotor and controller for each
		@@ -282,6 +332,9 @@ class DataLog:
		for arg in self._args:
		setattr(self, arg, None)
		setattr(self, '_' + str(arg), [])
		self._target = dict(
		position=[], velocity=[], orientation=[], rate=[]
		)
		self.vehicle = vehicle
		self.controller = controller

		@@ -300,6 +353,18 @@ class DataLog:
		self._times.append(self.vehicle.t)
		if self.controller is not None:
		self._actions.append(self.controller.action)
		self._target['position'].append(
		self.controller.reference[:3]
		)
		self._target['velocity'].append(
		np.concatenate((self.controller.ctrl_v.reference, self.controller.ctrl_z.action))
		)
		self._target['orientation'].append(
		np.concatenate((self.controller.ctrl_a.reference, self.controller.reference[3:4]))
		)
		self._target['rate'].append(
		self.controller.ctrl_r.reference
		)
		for key, value in kwargs.items():
		getattr(self, '_' + key).append(value)

		@@ -312,21 +377,70 @@ class DataLog:
		self._make_arrays()
		self._states = []
		self._actions = []
		self._times = []
		for arg in self._args:
		setattr(self, '_' + arg, [])
		self._target = dict(
		position=[], velocity=[], orientation=[], rate=[]
		)


		def append(self, log: 'DataLog', relative=True):
		assert set(self._args) == set(log._args), 'Inconsistent logged variables'
		old_len = len(self)
		self._states.extend(log._states)
		self._actions.extend(log._actions)
		self._times.extend(log._times)
		if self._arrayed and log._arrayed:
		self.states = np.concatenate((self.states, log.states), dtype=self.vehicle.dtype)
		self.actions = np.concatenate((self.actions, log.actions), dtype=self.vehicle.dtype)
		self.times = np.concatenate((self.times, log.times), dtype=self.vehicle.dtype)

		for arg in self._args:
		lst = getattr(self, '_' + arg, [])
		otherlst = getattr(log, '_' + arg, [])
		lst.extend(otherlst)
		setattr(self, '_' + arg, lst)
		if self._arrayed and log._arrayed:
		arr = getattr(self, arg)
		otherarr = getattr(log, arg)
		arr = np.concatenate((arr, otherarr), dtype=self.vehicle.dtype)
		setattr(self, arg, arr)

		self._target = {name: lst + log._target[name] for name, lst in self._target.items()}
		if self._arrayed and log._arrayed:
		d = {}
		for k in self._target.keys():
		a1 = getattr(self.target, k)
		a2 = getattr(log.target, k)
		a = np.concatenate((a1, a2), dtype=self.vehicle.dtype)
		d[k] = a
		self.target = SimpleNamespace(**d)

		if relative:
		last_time = self.times[old_len - 1]
		# last_pos = self.position[old_len - 1]
		if not self._arrayed:
		for i in range(old_len, len(self)):
		# self._states[i][:3] += last_pos
		self._times[i] += last_time
		elif self._arrayed:
		# self.states[old_len:,:3] += last_pos
		self.times[old_len:] += last_time


		def _make_arrays(self):
		def _make_arrays(self, relative_to=None):
		"""
		Convert python list to array and put up a flag that all arrays are up
		to date.
		"""
		if not self._arrayed:
		self.states = np.asarray(self._states)
		self.actions = np.asarray(self._actions)
		self.times = np.asarray(self._times)
		self.states = np.asarray(self._states, self.vehicle.dtype)
		self.actions = np.asarray(self._actions, self.vehicle.dtype)
		self.times = np.asarray(self._times, self.vehicle.dtype)
		for arg in self._args:
		setattr(self, arg, np.asarray(getattr(self, '_' + arg)))
		setattr(self, arg, np.asarray(getattr(self, '_' + arg), self.vehicle.dtype))
		self.target = SimpleNamespace(**{k: np.asarray(v, self.vehicle.dtype) for k, v in self._target.items()})
		self._arrayed = True

multirotor/trajectories.py

+25 −61

Changes for multirotor/trajectories.py: 25 added lines, 61 removed lines.
Original line number	Diff line number	Diff line
		from collections import namedtuple
		from typing import Iterable

		import numpy as np
		@@ -6,53 +5,6 @@ from pyscurve import ScurvePlanner
		from pyscurve.trajectory import PlanningError

		from multirotor.simulation import Multirotor
		from multirotor.vehicle import VehicleParams, SimulationParams, PropellerParams
		from multirotor.helpers import control_allocation_matrix
		from multirotor.physics import torque


		def get_vehicle_ability(
		vp: VehicleParams, sp: SimulationParams,
		max_tilt: float=np.pi/12,
		max_angular_acc: float=5,
		max_rads: float=600
		):
		alloc, alloc_inverse = control_allocation_matrix(vp)
		I_roll, I_pitch, I_yaw = vp.inertia_matrix.diagonal()
		n = len(vp.propellers)

		thrusts = [p.k_thrust * max_rads**2 for p in vp.propellers]
		max_f = sum(thrusts)
		max_acc_z = (max_f - (vp.mass * sp.g)) / vp.mass

		# Thrust produced at max_tilt to keep vehicle altitude constant.
		# Use that to calculate lateral component of thrust and lateral acceleration
		tilt_hover_thrust = (vp.mass * sp.g) / (n * np.cos(max_tilt))
		# Thrust is limited by what is possible by the propellers
		tilt_hover_thrust = min(max_f, tilt_hover_thrust)
		max_acc_xy = n * tilt_hover_thrust * np.sin(max_tilt) / vp.mass

		thrust_vec = np.zeros((3, n))
		thrust_vec[2] = np.asarray(thrusts)
		k_drag_vec = np.asarray([p.k_drag for p in vp.propellers])
		inertia_vec = np.asarray([p.moment_of_inertia for p in vp.propellers])
		torques = torque(
		position_vector=vp.propeller_vectors,
		force=thrust_vec,
		clockwise=np.asarray(vp.clockwise).astype(float),
		drag_coefficient=k_drag_vec,
		moment_of_inertia=inertia_vec,
		prop_angular_acceleration=0,
		prop_angular_velocity=max_rads
		)
		I = vp.inertia_matrix.diagonal()
		ang_acc = None

		res = dict(
		max_acc_xy=max_acc_xy,
		max_acc_z=max_acc_z,
		)
		return res



		@@ -95,8 +47,8 @@ class Trajectory:
		provided. For e.g. resolution=2 and points=(0,0,0), (10,0,0) will
		create intermediate points a distance 2 apart.
		"""
		self.points = points
		self.vehicle = vehicle
		self.points = np.asarray(points, self.vehicle.dtype)
		self.proximity = proximity
		self.resolution = resolution
		self.ref = None
		@@ -110,15 +62,26 @@ class Trajectory:
		return self._points[i]


		def get_params(self):
		return dict(
		proximity=self.proximity, resolution=self.resolution
		)


		def set_params(self, **params):
		self.proximity = params.get('proximity', self.proximity)
		self.proximity = params.get('resolution', self.resolution)


		def __iter__(self):
		self._points = self.generate_trajectory(self.vehicle.position)
		if self.proximity is not None:
		for i in range(len(self)):
		while np.linalg.norm((self.vehicle.position - self[i])) >= self.proximity:
		for i in range(1, len(self)):
		while not self.reached(self[i]):
		self.ref = self[i]
		yield self.ref, None
		else:
		for i in range(len(self)):
		for i in range(1, len(self)):
		self.ref = self[i]
		yield self.ref, None

		@@ -126,6 +89,8 @@ class Trajectory:
		def generate_trajectory(self, curr_pos=None):
		if curr_pos is not None:
		points = [curr_pos, *self.points]
		else:
		points = self.points
		points = np.asarray([p[:3] for p in points])
		if self.resolution is not None:
		_points = []
		@@ -158,11 +123,11 @@ class GuidedTrajectory:
		# https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/control.cpp#L286

		def __init__(self, vehicle: Multirotor, waypoints: Iterable[np.ndarray],
		interval: int=10, proximity: float=2., max_velocity: float=7., max_acceleration: float=3.,
		steps: int=10, proximity: float=2., max_velocity: float=7., max_acceleration: float=3.,
		max_jerk: float=100, turn_factor: float=(1/np.sqrt(2))) -> None:
		self.vehicle = vehicle
		self.waypoints = np.asarray(waypoints)
		self.interval = int(interval)
		self.steps = int(steps)
		self.proximity = float(proximity)
		self.max_velocity = float(max_velocity)
		self.max_acceleration = float(max_acceleration)
		@@ -173,10 +138,10 @@ class GuidedTrajectory:


		def _setup(self):
		if self.interval is None:
		if self.steps is None:
		dt = self.vehicle.simulation.dt
		# default run at 100Hz
		self.interval = int(max(1, 0.01 / dt))
		self.steps = int(max(1, 0.01 / dt))


		def __iter__(self):
		@@ -188,11 +153,10 @@ class GuidedTrajectory:
		planner = ScurvePlanner(debug=False)
		self._ref = None
		for k, wp in enumerate(self.waypoints):
		print('WP @ i=%d' % i, wp)
		replan = True # new waypoint, so replan immediately
		while not self.reached(wp):
		# Refresh trajectory plan
		if i % self.interval == 0 or replan:
		if i % self.steps == 0 or replan:
		r0 = rx = self.vehicle.position[:2]
		v0 = vx = self.vehicle.velocity[:2]
		r1 = wp[:2]
		@@ -224,7 +188,7 @@ class GuidedTrajectory:
		j_max=self.max_jerk
		)
		self.trajs.append(self.trajectory)
		replan = False # once replanned, wait for next waypoint or interval
		replan = False # once replanned, wait for next waypoint or steps
		j = 0 # reset steps since replan
		except PlanningError as e:
		# If planning error was on a new point/or replanning was
		@@ -236,9 +200,9 @@ class GuidedTrajectory:
		# print('Err: %s' % e)
		# raise e
		pass
		replan = True # after error, replan at next interval
		replan = True # after error, replan at next steps
		# Simulate trajectory plan over time
		for _ in range(self.interval):
		for _ in range(self.steps):
		target = self.trajectory(self.vehicle.simulation.dt * j)
		point = target[:, 2]
		velocity = target[:, 1]

multirotor/visualize.py

+44 −36

File changed.

Preview size limit exceeded, changes collapsed.

Admin message