Commit a55e2df3 authored by Ibrahim's avatar Ibrahim
Browse files

Controller improvements; 

Removed hardcoded constraint on cascaded PID components being P-only in some cases.
Added vertical velocity constraint.
Added predict() method for compatibility with RL code.
parent 61ddf27d

multirotor/controller/pid.py

+26 −12
Changes for multirotor/controller/pid.py: 26 added lines, 12 removed lines.
Original line number Diff line number Diff line
@@ -96,7 +96,8 @@ class PIDController: 




    def step(

        self, reference: np.ndarray, measurement: np.ndarray, dt: float=1., ref_is_error: bool=False

        self, reference: np.ndarray, measurement: np.ndarray, dt: float=1.,

        ref_is_error: bool=False

    ) -> np.ndarray:

        """

        Calculate the output, based on the current measurement and the reference
@@ -153,9 +154,8 @@ class PosController(PIDController): 


    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_i = np.ones(2) * np.asarray(self.k_i)

        self.k_d = np.ones(2) * np.asarray(self.k_d)

        if self.leashing or self.square_root_scaling:

            self.k_p[:] = 0.5 * self.max_jerk / self.max_acceleration

        super().__post_init__()
@@ -182,6 +182,7 @@ class PosController(PIDController): 
        # inertial frame velocity

        err = reference - measurement

        err_len = np.linalg.norm(err)

        # TODO check conditional logic

        if self.leashing:

            err_unit = err / (err_len + 1e-6)

            err_len = min(err_len, self.leash)
@@ -198,9 +199,10 @@ class PosController(PIDController): 
            velocity = super().step(reference, measurement, dt=dt)

        # convert to body-frame velocity

        roll, pitch, yaw = self.vehicle.orientation

        cos, sin = np.cos(yaw), np.sin(yaw)

        rot = np.asarray([

            [np.cos(yaw),   np.sin(yaw)],

            [-np.sin(yaw),  np.cos(yaw)],

            [cos,   sin],

            [-sin, cos],

        ])

        ref_velocity = rot @ velocity

        ref_velocity_mag = np.linalg.norm(ref_velocity)
@@ -264,9 +266,8 @@ class AttController(PIDController): 


    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)

        self.k_i = np.ones(3) * np.asarray(self.k_i)

        self.k_d = np.ones(3) * np.asarray(self.k_d)

        if self.square_root_scaling:

            self.k_p[:] = self.max_jerk / self.max_acceleration

        super().__post_init__()
@@ -322,12 +323,12 @@ class RateController(PIDController): 
        mea = euler_to_angular_rate(measurement, self.vehicle.orientation)

        # mea = measurement

        # prescribed change in velocity i.e. angular acc

        acceleration = np.clip(

        self.action = np.clip(

            super().step(reference=ref, measurement=mea, dt=dt),

            -self.max_acceleration, self.max_acceleration

        )

        # torque = moment of inertia . angular_acceleration

        # self.action = self.vehicle.params.inertia_matrix.dot(acceleration)

        self.action = self.vehicle.params.inertia_matrix.dot(self.action)

        return self.action
@@ -343,6 +344,7 @@ class AltController(PIDController): 
    """



    vehicle: Multirotor

    max_velocity: float = 5





    def __post_init__(self):
@@ -353,6 +355,14 @@ class AltController(PIDController): 
        super().__post_init__()





    def step(

        self, reference: np.ndarray, measurement: np.ndarray, dt: float = 1

    ) -> np.ndarray:

        self.action = super().step(reference, measurement, dt)

        self.action = np.clip(self.action, a_min=-self.max_velocity, a_max=self.max_velocity)

        return self.action





            

@dataclass

class AltRateController(PIDController):
@@ -438,7 +448,7 @@ class Controller: 




    def reset(self):

        self.action = None

        self.action = np.zeros(4, self.vehicle.dtype)

        self.thrust = None

        self.torques = None

        self._ref_vel = np.zeros(2, self.vehicle.dtype)
@@ -498,3 +508,7 @@ class Controller: 
        self.n += 1

        self.t = self.vehicle.t

        return self.action





    def predict(self, ref, deterministic=True):

        return self.step(reference=ref)

multirotor/helpers.py

+1 −0
Changes for multirotor/helpers.py: 1 added line, 0 removed lines.
Original line number Diff line number Diff line
@@ -2,6 +2,7 @@ from typing import Callable, Iterable, Tuple 


import numpy as np

from scipy.optimize import fsolve

from tqdm.autonotebook import tqdm



from .vehicle import PropellerParams, VehicleParams

multirotor/trajectories.py

+5 −2
Changes for multirotor/trajectories.py: 5 added lines, 2 removed lines.
Original line number Diff line number Diff line
@@ -99,6 +99,7 @@ class Trajectory: 
        self.vehicle = vehicle

        self.proximity = proximity

        self.resolution = resolution

        self.ref = None

        



    def __len__(self):
@@ -114,10 +115,12 @@ class Trajectory: 
        if self.proximity is not None:

            for i in range(len(self)):

                while np.linalg.norm((self.vehicle.position - self[i])) >= self.proximity:

                        yield self[i], None

                        self.ref = self[i]

                        yield self.ref, None

        else:

            for i in range(len(self)):

                        yield self[i], None

                self.ref = self[i]

                yield self.ref, None





    def generate_trajectory(self, curr_pos=None):

multirotor/visualize.py

+3 −0
Changes for multirotor/visualize.py: 3 added lines, 0 removed lines.
Original line number Diff line number Diff line
@@ -7,12 +7,15 @@ import time 


import numpy as np

import matplotlib.pyplot as plt

from tqdm.autonotebook import tqdm

from matplotlib.animation import FuncAnimation

from mpl_toolkits.mplot3d import Axes3D

from mpl_toolkits.mplot3d.art3d import Line3D



from .coords import body_to_inertial, direction_cosine_matrix

from .simulation import Multirotor

from .trajectories import Trajectory, GuidedTrajectory

from .controller import Controller

from .helpers import DataLog