Commit b1619f33 authored by Ibrahim's avatar Ibrahim
Browse files

Custom logging support; 

simulation.py: reversed order of time/state in dxdt_* methods so the controls library can use them to make system objects,
helpers.py: Added DataLog class which will track state/action variables per time step,
trajectories.py: Added loitering support so points specified as (x,y,z,T) will repeat for T steps in trajectory,
control.py: Added  attribute to all controllers to keep track of their most recent action
parent 4a906b17

README.md

+16 −0
Original line number Diff line number Diff line
@@ -13,3 +13,19 @@ Please cite this as: 
    organization={IEEE}

}

```



## Installation



Install from the Python Package Index (PyPI):



```

pip install multirotor

```



Or, clone repository and install for development. This will allow you to change the code of the package so the changes show up when you `import multirotor` in other projects.



```

git clone https://github.com/hazrmard/multirotor.git

cd multirotor

pip install -e .

```
 
 No newline at end of file

control.py

+13 −4
Original line number Diff line number Diff line
@@ -37,6 +37,7 @@ class PIDController: 
        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.state = None

        if self.max_err_i is None:

            self.max_err_i = np.inf
@@ -46,6 +47,7 @@ class PIDController: 
        self.err_p *= 0

        self.err_i *= 0

        self.err_d *= 0

        self.state = None





    def step(self, reference: np.ndarray, measurement: np.ndarray) -> np.ndarray:
@@ -73,7 +75,8 @@ class PIDController: 
        )

        self.err_d = (err - self.err) / self.dt

        self.err = err

        return self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d

        self.state = self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d

        return self.state
@@ -91,7 +94,7 @@ class PosController(PIDController): 
    """



    vehicle: Multirotor

    max_tilt: float = np.pi / 15

    max_tilt: float = np.pi / 18

    "Maximum tilt angle in radians"

    max_velocity: float = 1.0

    "Maximum velocity in m/s"
@@ -128,6 +131,7 @@ class PosController(PIDController): 
        # ctrl[1] -> y dir -> roll -> lateral

        ctrl[0:2] = np.clip(ctrl[0:2], a_min=-self.max_tilt, a_max=self.max_tilt)

        ctrl[1] *= -1 # +y motion requires negative roll

        self.state = ctrl

        return ctrl # desired pitch, roll
@@ -161,7 +165,8 @@ class AttController(PIDController): 
        # prescribed change in velocity i.e. angular acceleration

        ctrl = super().step(reference=ref_delta, measurement=mea_delta)

        # torque = moment of inertia . angular_acceleration

        return self.vehicle.params.inertia_matrix.dot(ctrl)

        self.state = self.vehicle.params.inertia_matrix.dot(ctrl)

        return self.state
@@ -190,6 +195,7 @@ class AltController(PIDController): 
                    ctrl / (np.cos(roll) * np.cos(pitch))

                ) + \

                self.vehicle.weight

            self.state = ctrl

            return ctrl # thrust force
@@ -207,12 +213,14 @@ class Controller: 
        self.ctrl_p = ctrl_p

        self.ctrl_a = ctrl_a

        self.ctrl_z = ctrl_z

        self.state = np.zeros(4)

        self.vehicle = self.ctrl_a.vehicle

        assert self.ctrl_a.vehicle is self.ctrl_p.vehicle, "Vehicle instances different."

        assert self.ctrl_a.vehicle is self.ctrl_z.vehicle, "Vehicle instances different."





    def reset(self):

        self.state = np.zeros(4)

        self.ctrl_a.reset()

        self.ctrl_p.reset()

        self.ctrl_z.reset()
@@ -224,4 +232,5 @@ class Controller: 
        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])

        return np.asarray([thrust, *torques])
 
        self.state = np.asarray([thrust, *torques])

        return self.state

helpers.py

+147 −1
Original line number Diff line number Diff line
@@ -193,6 +193,22 @@ def moment_of_inertia_disk(m: float, r: float) -> float: 




def control_allocation_matrix(params: VehicleParams) -> Tuple[np.ndarray, np.ndarray]:

    """

    Calculate the control allocation matrix such that:



        action = matrix @ [thrust, torque_x, torque_y, torque_z]



    Parameters

    ----------

    params : VehicleParams

        The vehicle parameters for which to compute the matrix



    Returns

    -------

    Tuple[np.ndarray, np.ndarray]

        The allocation matrix and its inverse. If no inverse exists, returns

        the Moore-Penrose Pseudo-inverse.

    """

    alloc = np.zeros((4, len(params.propellers))) #[Fz, Mx, My, Mz] x n-Propellers

    x = params.distances * np.cos(params.angles)

    y = params.distances * np.sin(params.angles)
@@ -203,3 +219,133 @@ def control_allocation_matrix(params: VehicleParams) -> Tuple[np.ndarray, np.nda 
        alloc[3, i] = p.k_drag * params.clockwise[i]    # torque about z-axis

    alloc_inverse = np.linalg.pinv(alloc)

    return alloc, alloc_inverse







class DataLog:

    """

    Records state and action variables for a multirotor and controller for each

    simulation step.

    """

    def __init__(

        self, vehicle: 'Multirotor'=None, controller: 'Controller'=None,

        *other_vars

    ):

        """

        Parameters

        ----------

        vehicle : Multirotor, optional

            The Multirotor to track, by default None

        controller : Controller, optional

            The controller instance to track, by default None

        """

        return self.track(vehicle, controller, *other_vars)





    def track(self, vehicle, controller, *other_vars):

        """

        Register Multirotor and Controller instances to track, along with names

        of any other variables to be manually added.



        >>> DataLog.track(Multirotor(), Controller(), 'error')



        Parameters

        ----------

        vehicle : Multirotor

            The vehicle to track.

        controller : Controller

            The controller to track.

        """

        self._states_names = ('x','y','z',

                              'vx','vy','vz',

                              'roll','pitch','yaw',

                              'xrate', 'yrate', 'zrate')

        self._action_names = ('thrust', 'torque_x', 'torque_y', 'torque_z')

        self._arrayed = False

        self._states = []

        self.states = None

        self._actions = []

        self.actions = None

        self._args = other_vars

        for arg in self._args:

            setattr(self, arg, None)

            setattr(self, '_' + str(arg), [])

        self.vehicle = vehicle

        self.controller = controller



        

    def log(self, **kwargs):

        """

        Add the state and action variables from the Multirotor and Controller.

        Any keyword arguments should already have been registered in `track()`

        and their values are now appended to the list.



        >>> DataLog.log(error=5)

        """

        self._arrayed = False

        if self.vehicle is not None:

            self._states.append(self.vehicle.state)

        if self.controller is not None:

            self._actions.append(self.controller.state)

        for key, value in kwargs.items():

            getattr(self, '_' + key).append(value)



            

    def done_logging(self):

        """

        Indicate that no more logs are goingto be put so the python lists are converted

        to numpy arrays and discarded.

        """

        self._make_arrays()

        self._states = []

        self._actions = []

        for arg in self._args:

            setattr(self, '_' + arg, [])



            

    def _make_arrays(self):

        """

        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)

            for arg in self._args:

                setattr(self, arg, np.asarray(getattr(self, '_' + arg)))

        self._arrayed = True



        

    @property

    def position(self):

        self._make_arrays()

        return self.states[:, 0:3]

    @property

    def x(self):

        return self.position[:, 0].reshape(-1)

    @property

    def y(self):

        return self.position[:, 1].reshape(-1)

    @property

    def z(self):

        return self.position[:, 2].reshape(-1)

    @property

    def velocity(self):

        self._make_arrays()

        return self.states[:, 3:6]

    @property

    def orientation(self):

        self._make_arrays()

        return self.states[:, 6:9]

    @property

    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]
 
 No newline at end of file

setup.cfg

+3 −0
Original line number Diff line number Diff line
@@ -26,3 +26,6 @@ where = . 
DEV =

    twine

    build

    sphinx

    sphinx-autoapi

    numpydoc

simulation.py

+36 −10
Original line number Diff line number Diff line
@@ -234,6 +234,7 @@ class Multirotor: 
        self.propellers: List[Propeller] = None

        self.propeller_vectors: np.ndarray = None

        self.t: float = 0.

        self.dxdt_decimals = max(1, 1 - int(np.log10(self.simulation.dt)))

        self.propellers = []

        for params in self.params.propellers:

            self.propellers.append(Propeller(params, self.simulation))
@@ -359,17 +360,17 @@ class Multirotor: 
        return forces, torques





    def dxdt_dynamics(self, x: np.ndarray, t: float, u: np.ndarray):

    def dxdt_dynamics(self, t: float, x: np.ndarray, u: np.ndarray):

        """

        Calculate the rate of change of state given the dynamics (forces, torques)

        acting on the system.



        Parameters

        ----------

        x : np.ndarray

            State of the vehicle.

        t : float

            Time. Currently this function is time invariant.

        x : np.ndarray

            State of the vehicle.

        u : np.ndarray

            A 6-vector of forces and torques.
@@ -387,20 +388,20 @@ class Multirotor: 
        xdot = apply_forces_torques(

            u[:3], u[3:], x, self.simulation.g,

            self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse)

        return np.around(xdot, 3)

        return np.around(xdot, self.dxdt_decimals)





    def dxdt_speeds(self, x: np.ndarray, t: float, u: np.ndarray):

    def dxdt_speeds(self, t: float, x: np.ndarray, u: np.ndarray):

        """

        Calculate the rate of change of state given the propeller speeds on the

        system (rad/s).



        Parameters

        ----------

        x : np.ndarray

            State of the vehicle.

        t : float

            Time. Currently this function is time invariant.

        x : np.ndarray

            State of the vehicle.

        u : np.ndarray

            A p-vector of propeller speeds (rad/s), where p=number of propellers.
@@ -416,7 +417,7 @@ class Multirotor: 
        xdot = apply_forces_torques(

            forces, torques, x, self.simulation.g,

            self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse)

        return np.around(xdot, 3)

        return np.around(xdot, self.dxdt_decimals)





    def step_dynamics(self, u: np.ndarray) -> np.ndarray:
@@ -438,7 +439,7 @@ class Multirotor: 
        self.t += self.simulation.dt

        self.state = odeint(

            self.dxdt_dynamics, self.state, (0, self.simulation.dt), args=(u,),

            rtol=1e-4, atol=1e-4

            rtol=1e-4, atol=1e-4, tfirst=True

        )[-1]

        self.state = np.around(self.state, 4)

        # TODO: inverse solve for speed = forces to set propeller speeds
@@ -465,7 +466,7 @@ class Multirotor: 
        self.t += self.simulation.dt

        self.state = odeint(

            self.dxdt_speeds, self.state, (0, self.simulation.dt), args=(u,),

            rtol=1e-4, atol=1e-4

            rtol=1e-4, atol=1e-4, tfirst=True

        )[-1]

        self.state = np.around(self.state, 4)

        for u_, prop in zip(u, self.propellers):
@@ -497,3 +498,28 @@ class Multirotor: 
            np.clip(self.alloc_inverse @ vec, a_min=0., a_max=None)

        )





    def nonlinear_dynamics_controls_system(self):

        import control

        sys = control.NonlinearIOSystem(

            updfcn=self.dxdt_dynamics,

            inputs=['fx','fy','fz','tx','ty','tz'],

            outputs=['x','y','z',

                    'vx','vy','vz',

                    'roll','pitch','yaw',

                    'xrate', 'yrate', 'zrate']

        )

        return sys

    



    def nonlinear_speeds_controls_system(self):

        import control

        sys = control.NonlinearIOSystem(

            updfcn=self.dxdt_speeds,

            inputs=['w%d' % i for i in range(len(self.propellers))],

            outputs=['x','y','z',

                    'vx','vy','vz',

                    'roll','pitch','yaw',

                    'xrate', 'yrate', 'zrate']

        )

        return sys