Initial commit for drone 2D drone2D_model

drone2D/drone2D_model.py

+import yaml
+from scipy.integrate import solve_ivp
+
+class Drone2D:
+
+    def __init__(self, file="drone2D.yaml"):
+        with open(file, 'r') as stream:
+            drone_data = yaml.load(stream)
+        
+            self.G = 9.81
+            self.MASS = drone_data["MASS"]
+            self.INERTIA = drone_data["INERTIA"]
+            self.LENGTH = drone_data["LENGTH"]
+            self.HEIGHT = drone_data["HEIGHT"]
+
+    def dynamics(self, state: list, inp: list):
+        """
+        ODE system for the 2D drone.
+
+            Parameters:
+                state = [x, y, theta, vx, vy, vtheta]
+                input = [thrust1, thrust2]
+            
+            Returns:
+                list of derivatives
+        """
+
+        x, y, theta, vx, vy, vtheta = state
+        T1, T2 = inp
+
+        ax = - (T1 + T2) * theta / self.MASS
+        ay = ( (T1 + T2) / self.MASS ) - self.G
+        atheta = (T2 - T1) * self.LENGTH / self.INERTIA
+
+        return [vx, vy, vtheta, ax, ay, atheta]
+        
+        
+    def step(self, state, inp, dt):
+        """
+        Step the time for the 2D drone.
+
+            Parameters:
+                initial state = [x, y, theta, vx, vy, vtheta]
+                input = [thrust1, thrust2]
+                time step (scalar)
+            
+            Returns:
+                final state
+        """
+
+        func = lambda t, s: self.dynamics(s, inp)
+        solution = solve_ivp(func, [0, dt], state)
+
+        return solution.y[-1]
+
+