Commit b135ffa4 authored by Ibrahim's avatar Ibrahim
Browse files

Trajectory class for path generation; Alt PID tracks world z-velocity

parent 5632c5c4
......@@ -61,10 +61,22 @@ class PIDController:
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):
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):
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
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
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
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]
raise ValueError('Vehicle must be provided if a proximity value is given.')
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment