transitioning to ardupilot logic with trajectory planning. Work in Progress. (e69e6ee9) · Commits · Ibrahim Ahmed / multirotor

multirotor/controller/init.py

0 → 100644

+10 −0

Original line number	Diff line number	Diff line
		from .pid import (
		PIDController,
		PosController,
		VelController,
		AttController,
		RateController,
		AltController,
		AltRateController,
		Controller
		)

multirotor/controller/guided.py

0 → 100644

+23 −0

Original line number	Diff line number	Diff line
		"""
		Guided mode control. Generates x,y,z,yaw references for PID controllers.
		"""

		import numpy as np



		class GuidedTrajectory:


		def __init__(self, vehicle, controller) -> None:
		self.vehicle = vehicle
		self.controller = controller
		self.waypoints = []


		def add_waypoint(self, waypoint: np.ndarray):
		self.waypoints.append(np.asarray(waypoint))


		def __iter__(self):
		pass
		No newline at end of file

multirotor/controller.py→multirotor/controller/pid.py

+262 −52

Original line number	Diff line number	Diff line
		@@ -2,10 +2,46 @@ from dataclasses import dataclass

		import numpy as np
		from scipy.integrate import trapezoid

		from .simulation import Multirotor
		from .coords import euler_to_angular_rate

		from numba import njit

		from ..simulation import Multirotor
		from ..coords import euler_to_angular_rate

		# See more:
		# https://archive.ph/3Aco3
		# https://ardupilot.org/dev/docs/apmcopter-code-overview.html
		# https://ardupilot.org/copter/docs/traditional-helicopter-control-system.html
		# https://ardupilot.org/dev/docs/apmcopter-programming-attitude-control-2.html
		# https://www.youtube.com/watch?v=-PC69jcMizA


		@njit
		def sqrt_control(err: float, k_p: float, derr_dt2_lim: float, dt: float):
		# piece-wise P-controller
		# see: https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/control.cpp#L387
		correction_rate = 0.
		if derr_dt2_lim <= 0:
		correction_rate = err * k_p
		elif k_p == 0:
		if err > 0:
		correction_rate = np.sqrt(2. * derr_dt2_lim * err)
		if err < 0:
		correction_rate = -np.sqrt(2. * derr_dt2_lim * (-err))
		else:
		correction_rate = 0.
		else:
		linear_dist = derr_dt2_lim / k_p**2
		if err > linear_dist:
		correction_rate = np.sqrt(2. * derr_dt2_lim * (err - (linear_dist / 2.)))
		elif err < -linear_dist:
		correction_rate = -np.sqrt(2. * derr_dt2_lim * (-err - (linear_dist / 2.)))
		else:
		correction_rate = err * k_p
		if dt != 0.:
		abs_val = np.abs(err) / dt
		return min(max(-abs_val, correction_rate), abs_val)
		else:
		return correction_rate


		@dataclass
		@@ -34,12 +70,14 @@ class PIDController:

		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.err_p = np.atleast_1d(np.zeros_like(self.k_p))
		self.err_i = np.atleast_1d(np.zeros_like(self.k_i))
		self.err_d = np.atleast_1d(np.zeros_like(self.k_d))
		self.err = np.atleast_1d(np.zeros_like(self.k_p))
		if self.max_err_i is None:
		self.max_err_i = np.inf
		else:
		self.max_err_i = np.asarray(self.max_err_i, dtype=self.err.dtype)


		def reset(self):
		@@ -59,7 +97,9 @@ class PIDController:
		))


		def step(self, reference: np.ndarray, measurement: np.ndarray) -> np.ndarray:
		def step(
		self, reference: np.ndarray, measurement: np.ndarray, ref_is_error: bool=False
		) -> np.ndarray:
		"""
		Calculate the output, based on the current measurement and the reference
		signal.
		@@ -70,12 +110,17 @@ class PIDController:
		The reference signal(s) to track. Can be a number or an array.
		measurement : np.ndarray
		The actual measurement(s).
		ref_is_error: bool
		Whether to interpret the reference input as the error.

		Returns
		-------
		np.ndarray
		The action signal.
		"""
		if ref_is_error:
		err = reference
		else:
		err = reference - measurement
		self.err_p = err
		self.err_i = np.clip(
		@@ -92,21 +137,70 @@ class PIDController:
		@dataclass
		class PosController(PIDController):
		"""
		Position controller. Takes reference x/y position and outputs reference
		Position controller. Convert xy-waypoint into required xy-velocity in the body-frame.
		"""

		vehicle: Multirotor
		max_velocity: float = 5.0
		"Maximum velocity in m/s"
		max_acceleration: float = 10.0
		"Maximum acceleration in m/s/s"
		max_jerk: float = 100.0
		"Maximum jerk in m/s/s/s"
		square_root_scaling: bool = True
		"Whether to scale P-gain with the square root of the error"


		def __post_init__(self):
		self.k_p = np.ones(2) * np.asarray(self.k_p)
		# Att angle controller is strictly a P controller
		self.k_i = np.zeros(2) * np.asarray(self.k_i)
		self.k_d = np.zeros(2) * np.asarray(self.k_d)
		self.k_p_init = self.k_p.copy()
		super().__post_init__()


		def step(self, reference, measurement):
		# inertial frame velocity
		if self.square_root_scaling:
		err = reference - measurement
		velocity = np.zeros_like(err)
		err_len = np.linalg.norm(err)
		if err_len > 0.:
		k_p = 0.5 * self.max_jerk / self.max_acceleration
		velocity = sqrt_control(err[0], k_p, self.max_acceleration, self.dt)
		# scale velocity correction by error size
		velocity = (err / err_len) * velocity / self.dt
		else:
		velocity = super().step(reference, measurement) / self.dt
		# convert to body-frame velocity
		roll, pitch, yaw = self.vehicle.orientation
		rot = np.asarray([
		[np.cos(yaw), np.sin(yaw)],
		[-np.sin(yaw), np.cos(yaw)],
		])
		ref_velocity = rot @ velocity
		abs_max_vel = np.abs((ref_velocity / (np.linalg.norm(ref_velocity) + 1e-6)) * self.max_velocity)
		ref_velocity = np.clip(ref_velocity, a_min=-abs_max_vel, a_max=abs_max_vel)
		self.action = ref_velocity
		return self.action


		@dataclass
		class VelController(PIDController):
		"""
		Velocity controller. Takes reference x/y body-frame velocity 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
		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.
		Able to limit tilt angles when tracking reference waypoints.
		"""

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



		@@ -118,24 +212,8 @@ class PosController(PIDController):


		def step(self, reference, measurement):
		roll, pitch, yaw = self.vehicle.orientation
		delta_x, delta_y = reference - measurement
		rot = np.asarray([
		[np.cos(yaw), np.sin(yaw)],
		[-np.sin(yaw), np.cos(yaw)],
		])
		# convert reference x/y to body frame, given yaw
		# Using rotation matrix. For a positive yaw, the target x,y will appear
		# desired/reference change in x/y i.e. velocity
		ref_delta_xy = rot @ np.asarray([delta_x, delta_y])
		# TODO: Explicitly track velocity, instead of deltas
		ref_vel_xy = ref_delta_xy / self.vehicle.simulation.dt
		abs_max_vel = np.abs((ref_vel_xy / (np.linalg.norm(ref_vel_xy) + 1e-6)) * self.max_velocity)
		ref_vel_xy = np.clip(ref_vel_xy, a_min=-abs_max_vel, a_max=abs_max_vel)
		# actual/measured velocity
		mea_delta_xy = mea_vel_xy = self.vehicle.velocity[:2]
		# desired pitch, roll
		ctrl = super().step(reference=ref_vel_xy, measurement=mea_delta_xy)
		ctrl = super().step(reference, measurement)
		# ctrl[0] -> x dir -> pitch -> forward
		# ctrl[1] -> y dir -> roll -> lateral
		ctrl[0:2] = np.clip(ctrl[0:2], a_min=-self.max_tilt, a_max=self.max_tilt)
		@@ -147,8 +225,41 @@ class PosController(PIDController):

		@dataclass
		class AttController(PIDController):

		vehicle: Multirotor
		max_velocity: float = 5.0
		"Maximum velocity in m/s"
		max_acceleration: float = 10.0
		"Maximum acceleration in m/s/s"
		max_jerk: float = 100.0
		"Maximum jerk in m/s/s/s"
		square_root_scaling: bool = False
		"Whether to scale P-gain with the square root of the error"


		def __post_init__(self):
		self.k_p = np.ones(3) * np.asarray(self.k_p)
		# Att angle controller is strictly a P controller
		self.k_i = np.zeros(3) * np.asarray(self.k_i)
		self.k_d = np.zeros(3) * np.asarray(self.k_d)
		super().__post_init__()


		def step(self, reference, measurement):
		if self.square_root_scaling:
		err = reference - measurement
		k_p = self.max_jerk / self.max_acceleration
		acceleration = sqrt_control(err, k_p, self.max_jerk, self.dt)
		ctrl = super().step(reference=reference, measurement=measurement)
		self.action = ctrl
		return self.action



		@dataclass
		class RateController(PIDController):
		"""
		Attitude controller. Tracks reference roll, pitch, yaw angles and outputs
		Attitude rate controller. Tracks reference roll, pitch, yaw rates 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
		@@ -166,7 +277,7 @@ class AttController(PIDController):
		super().__post_init__()


		def step(self, reference, measurement):
		def step_(self, reference, measurement):
		# desired change in orientation i.e. angular velocity
		ref_delta = euler_to_angular_rate(reference - measurement, self.vehicle.orientation)
		# actual change in orientation
		@@ -178,11 +289,37 @@ class AttController(PIDController):
		return self.action


		def step__(self, reference, measurement):
		# desired change in orientation i.e. angular velocity
		ref = euler_to_angular_rate(reference, self.vehicle.orientation)
		# actual change in orientation
		mea = euler_to_angular_rate(measurement, self.vehicle.orientation)
		# prescribed change in angle i.e. angular rate
		ctrl = super().step(reference=ref, measurement=mea)
		# prescribed change in angular rate i.e. angular acceleration
		acc = ctrl - self.vehicle.angular_rate
		# torque = moment of inertia . angular_acceleration
		self.action = self.vehicle.params.inertia_matrix.dot(acc)
		return self.action


		def step(self, reference, measurement):
		# desired change in angular velocity
		ref = euler_to_angular_rate(reference, self.vehicle.orientation)
		# actual change in angular velocity
		mea = euler_to_angular_rate(measurement, self.vehicle.orientation)
		# prescribed change in velocity i.e. angular acc
		ctrl = super().step(reference=ref, measurement=mea)
		# torque = moment of inertia . angular_acceleration
		self.action = self.vehicle.params.inertia_matrix.dot(ctrl)
		return self.action



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

		Uses change in z-position as approximate vertical velocity. Compares against
		measured velocity. Outputs the change in velocity (acceleration) as thrust force,
		@@ -192,15 +329,29 @@ class AltController(PIDController):
		vehicle: Multirotor


		def __post_init__(self):
		self.k_p = np.ones(1) * np.asarray(self.k_p)
		# Alt controller is strictly a P controller
		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__()



		@dataclass
		class AltRateController(PIDController):
		"""
		Climb rate controller. Tracks z-velocity and outputs thrust force needed.
		"""

		vehicle: Multirotor


		def step(self, reference, measurement):
		roll, pitch, yaw = self.vehicle.orientation
		# desired change in z i.e. velocity
		ref_delta_z = reference - measurement
		# actual change in z
		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
		# change in velocity i.e. acceleration
		ctrl = super().step(reference=reference, measurement=measurement)
		# convert acceleration to required z-force, given orientation
		ctrl = self.vehicle.params.mass * (
		ctrl / (np.cos(roll) * np.cos(pitch))
		) + \
		@@ -215,25 +366,63 @@ 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)
		(x,y) --> Position --> Velocity --> Attitude --> Rate --> (Moments)
		(z) --> Altitude --> Velocity --> (Forces)
		"""

		def __init__(self, ctrl_p: PosController, ctrl_a: AttController, ctrl_z: AltController):
		def __init__(
		self,
		ctrl_p: PosController, ctrl_v: VelController,
		ctrl_a: AttController, ctrl_r: RateController,
		ctrl_z: AltController, ctrl_vz: AltRateController,
		interval: float=None
		):
		"""
		Parameters
		----------
		ctrl_p : PosController
		The position controller.
		ctrl_v : VelController
		The velocity controller.
		ctrl_a : AttController
		The attitude controller.
		ctrl_r : AttRateController
		The attitude rate controller.
		ctrl_z : AltController
		The altitude controller
		ctrl_vz : AltRateController
		The altitude rate controller
		interval : float, optional
		The time resolution of the 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
		new action will only be applied every 10 steps. by default None
		"""
		self.ctrl_p = ctrl_p
		self.ctrl_v = ctrl_v
		self.ctrl_a = ctrl_a
		self.ctrl_r = ctrl_r
		self.ctrl_z = ctrl_z
		self.ctrl_vz = ctrl_vz
		self.vehicle = self.ctrl_a.vehicle
		self.interval = self.ctrl_a.vehicle.simulation.dt if interval is None else interval
		self.interval_n = int(self.interval // self.ctrl_a.vehicle.simulation.dt)
		self.action = None
		self.n = 0
		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."
		assert self.ctrl_a.vehicle is self.ctrl_z.vehicle, "Vehicle instances different."
		assert self.ctrl_a.vehicle is self.ctrl_vz.vehicle, "Vehicle instances different."


		def reset(self):
		self.n = 0
		self.action = None
		self.ctrl_a.reset()
		self.ctrl_p.reset()
		self.ctrl_z.reset()
		return self.state


		@property
		@@ -243,11 +432,32 @@ class Controller:
		)


		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.action = np.asarray([thrust, *torques])
		def step(self, reference: np.ndarray, measurement=None, ref_is_error: bool=False):
		if self.n % self.interval_n != 0:
		self.n += 1
		return self.action
		# If the reference argument is the relative error, and not the absolute
		# value. Using the relationship error = reference - measurement to
		# get absolute reference values
		if ref_is_error:
		error = reference
		ref_xy = self.vehicle.position[:2] + error[:2]
		ref_z = self.vehicle.position[2] + error[2]
		ref_yaw = self.vehicle.orientation[2] + error[3]
		else:
		ref_xy = reference[:2]
		ref_z = reference[2]
		ref_yaw = reference[3]

		ref_vel_z = self.ctrl_z.step(ref_z, self.vehicle.position[2:])
		thrust = self.ctrl_vz.step(ref_vel_z, self.vehicle.world_velocity[2:])

		ref_vel = self.ctrl_p.step(ref_xy, self.vehicle.position[:2])
		pitch_roll = self.ctrl_v.step(ref_vel, self.vehicle.velocity[:2])
		ref_orientation = np.asarray([pitch_roll[1], pitch_roll[0], ref_yaw])
		ref_rate = self.ctrl_a.step(ref_orientation, self.vehicle.orientation)
		torques = self.ctrl_a.step(ref_rate, self.vehicle.euler_rate)

		self.action = np.asarray([thrust, torques])
		self.n += 1
		return self.action

multirotor/helpers.py

+0 −8

Original line number	Diff line number	Diff line
		@@ -357,12 +357,4 @@ class DataLog:
		def angular_rate(self):
		self._make_arrays()
		return self.states[:, 9:12]
		@property
		def thrust(self):
		self._make_arrays()
		return self.actions[:, :1].reshape(-1)
		@property
		def torques(self):
		self._make_arrays()
		return self.actions[:, 1:4]
		# TODO: add properties for controller state

multirotor/physics.py

+8 −4

Original line number	Diff line number	Diff line
		@@ -62,9 +62,11 @@ def torque(
		Parameters
		----------
		position_vector : np.ndarray
		Position vector of force, relative to center of mass.
		Position vector of force, relative to center of mass. If multiple vectors,
		should be in shape 3 x n_vectors.
		force : np.ndarray
		Force vector acting at that position (nominally thrust).
		Force vector acting at that position (nominally thrust). If multiple vectors,
		should be in shape 3 x n_vectors.
		moment_of_inertia : float
		Moment of inertia of the body.
		prop_angular_acceleration : float
		@@ -87,12 +89,14 @@ def torque(
		# yaw moments
		# tau = I . d omega/dt
		tau_rot = (
		# angular acceleration of propellers is assumed to be negligible
		# clockwise * moment_of_inertia * prop_angular_acceleration +
		clockwise * drag_coefficient * prop_angular_velocity**2
		)
		# tau = r x F
		tau = np.cross(position_vector, force)
		# print(moment_of_inertia, prop_angular_acceleration)
		# numba does not support axis arguments for cross(), so taking transpose and
		# then undoing it for result:
		tau = np.cross(position_vector.T, force.T).T
		tau[2] = tau[2] + tau_rot
		return tau

Admin message