Trajectory class for path generation; Alt PID tracks world z-velocity (b135ffa4) · Commits · Ibrahim Ahmed / multirotor

control.py

+35 −1

Original line number	Diff line number	Diff line
		@@ -61,10 +61,22 @@ class PIDController:

		@dataclass
		class PosController(PIDController):
		"""
		Position controller. Takes reference x/y position and outputs reference
		pitch and roll angles for x and y motion, respectively.

		Uses vector from current to reference position as an approximation of
		reference velocity. Compares against measured velocity. The deficit is used
		to change pitch and roll angles to increase and decrease velocity.

		Able to limit maximum velocity and tilt angles when tracking reference waypoints.
		"""

		vehicle: Multirotor
		max_tilt: float = np.pi / 15
		"Maximum tilt angle in radians"
		max_velocity: float = 1.0
		"Maximum velocity in m/s"



		@@ -104,6 +116,14 @@ class PosController(PIDController):

		@dataclass
		class AttController(PIDController):
		"""
		Attitude controller. Tracks reference roll, pitch, yaw angles and outputs
		the necessary moments about each x,y,z axes to achieve them.

		Uses change in orientation from measured to reference as approximate reference
		angular rate. Compares against measured angular rate. Outputs required change
		in angular rate (angular acceleration) as moments.
		"""

		vehicle: Multirotor

		@@ -129,6 +149,13 @@ class AttController(PIDController):

		@dataclass
		class AltController(PIDController):
		"""
		Altitude Controller. Tracks z-position and outputs thrust force needed.

		Uses change in z-position as approximate vertical velocity. Compares against
		measured velocity. Outputs the change in velocity (acceleration) as thrust force,
		given orientation of vehicle.
		"""

		vehicle: Multirotor

		@@ -137,7 +164,7 @@ class AltController(PIDController):
		# desired change in z i.e. velocity
		ref_delta_z = reference - measurement
		# actual change in z
		mea_delta_z = self.vehicle.velocity[2]
		mea_delta_z = self.vehicle.world_velocity[2]
		# change in delta_z i.e. change in velocity i.e. acceleration
		ctrl = super().step(reference=ref_delta_z, measurement=mea_delta_z)
		# change in z-velocity i.e. acceleration
		@@ -150,6 +177,13 @@ class AltController(PIDController):


		class Controller:
		"""
		The cascaded PID controller. Tracks position and yaw, and outputs thrust and
		moments needed.

		(x,y) --> Position Ctrl --> (Angles) --> Attitude Ctrl --> (Moments)
		(z) --> Attitude Ctrl --> (Forces)
		"""

		def __init__(self, ctrl_p: PosController, ctrl_a: AttController, ctrl_z: AltController):
		self.ctrl_p = ctrl_p

trajectories.py

0 → 100644

+90 −0

Original line number	Diff line number	Diff line
		import numpy as np
		from multirotor.simulation import Multirotor

		from multirotor.vehicle import VehicleParams, SimulationParams, PropellerParams
		from multirotor.helpers import control_allocation_matrix



		def get_vehicle_ability(
		vp: VehicleParams, sp: SimulationParams, max_tilt: float=np.pi/12
		):
		alloc, alloc_inverse = control_allocation_matrix(vp)
		I_roll, I_pitch, I_yaw = vp.inertia_matrix.diagonal()
		# T = I . d_omega
		# s = 0.5 d_omega t^2
		# time to reach max tilt (s):
		# t_m = sqrt(2 s / d_omega) = t = sqrt(2 s I / T)
		# max_torque = from propellers x geometry
		# t_m = np.sqrt(2 * max_tilt * vp.inertia_matrix.diagonal() / max_torque)
		# max lateral thrust
		# T_l = Thrust sin(max_tilt)
		# max lateral acc
		# a = T_l / m
		# avg velocity = dist / time
		# x / (t_m + sqrt(2 (x/2) m / T_l) + 2 t_m + sqrt(2 (x/2) m / T_l))
		# tilt acc half way tilt reverse dec half way to stop



		class Trajectory:
		"""
		Iterate over waypoints for a multirotor. The trajectory class can segment a
		list of waypoints into smaller sections and feed them to the controller when
		the vehicle is within a radius of its current waypoint.
		"""


		def __init__(
		self, points: np.ndarray, vehicle: Multirotor=None, proximity: float=None,
		resolution: float=None
		):
		"""
		Parameters
		----------
		points : np.ndarray
		A list of 3D coordinates.
		vehicle : Multirotor, optional
		The vehicle to track. Required if proximity is provided, by default None
		proximity : float, optional
		The distance from current waypoint at which to send the next waypoint,
		by default None. If None, simply iterates over provided points.
		resolution : float, optional
		The segmentation of the trajectory, by default None. If provided, it
		is the distance between intermediate points generated from the waypoints
		provided. For e.g. resolution=2 and points=(0,0,0), (10,0,0) will
		create intermediate points a distance 2 apart.
		"""
		# TODO: loiter in trajectory
		self.proximity = proximity
		self.vehicle = vehicle
		self.points = np.asarray(points)
		if resolution is not None:
		points = []
		for p1, p2 in zip(self.points[:-1], self.points[1:]):
		dist = np.linalg.norm(p2 - p1)
		num = int(dist / resolution) + 1
		points.extend(np.linspace(p1, p2, num=num, endpoint=True))
		self._points = points
		else:
		self._points = points


		def __len__(self):
		return len(self._points)


		def __getitem__(self, i: int):
		return self._points[i]


		def __iter__(self):
		if self.proximity is not None and self.vehicle is not None:
		for i in range(len(self)):
		while np.linalg.norm((self.vehicle.position - self[i])) >= self.proximity:
		yield self[i]
		elif self.proximity is None:
		for i in range(len(self)):
		yield self[i]
		else:
		raise ValueError('Vehicle must be provided if a proximity value is given.')

Admin message