From ce6129fd22da4a49dfbb068118fe71c960786816 Mon Sep 17 00:00:00 2001
From: Yangge Li <li213@illinois.edu>
Date: Mon, 4 Mar 2024 21:12:39 -0600
Subject: [PATCH] add quadrotor example

---
 demo/dryvr_demo/dryvr_agent.py    | 68 +++++++++++++++++++++++++++++++
 demo/dryvr_demo/origin_agent.py   |  1 -
 demo/dryvr_demo/quadrotor_demo.py | 39 ++++++++++++++++++
 3 files changed, 107 insertions(+), 1 deletion(-)
 create mode 100644 demo/dryvr_demo/dryvr_agent.py
 create mode 100644 demo/dryvr_demo/quadrotor_demo.py

diff --git a/demo/dryvr_demo/dryvr_agent.py b/demo/dryvr_demo/dryvr_agent.py
new file mode 100644
index 00000000..44568f53
--- /dev/null
+++ b/demo/dryvr_demo/dryvr_agent.py
@@ -0,0 +1,68 @@
+# Example agent.
+from typing import Tuple, List
+
+import numpy as np
+from scipy.integrate import odeint
+
+from verse.agents import BaseAgent
+from verse.map import LaneMap
+from verse.parser import ControllerIR
+
+class QuadrotorAgent(BaseAgent):
+    def __init__(self, id, code = None, file_name = None):
+        self.decision_logic: ControllerIR = ControllerIR.empty()
+        self.id = id
+        self.init_cont = None 
+        self.init_disc = None
+        self.static_parameters = None 
+        self.uncertain_parameters = None
+
+    def dynamics(self, x, t):
+        x1, x2, x3,\
+        x4, x5, x6,\
+        x7, x8, x9,\
+        x10, x11, x12, u1 = x 
+
+        u2 = 0
+        u3 = 0
+
+        g = 9.81
+        R = 0.1 
+        l = 0.5
+        Mrotor = 0.1
+        M = 1 
+        m = M+4*Mrotor 
+
+        Jx = 2/5*M*R**2+2*l**2*Mrotor 
+        Jy = Jx 
+        Jz = 2/5*M*R**2+4*l**2*Mrotor 
+
+        F = m*g-10*(x3-u1)+3*x6 
+        tau_phi = -(x7-u2)-x10 
+        tau_theta = -(x8-u3)-x11 
+        tau_psi  = 0 
+
+        dx1 = np.cos(x8)*np.cos(x9)*x4+(np.sin(x7)*np.sin(x8)*np.cos(x9)-np.cos(x7)*np.sin(x9))*x5+(np.cos(x7)*np.sin(x8)*np.cos(x9)+np.sin(x7)*np.sin(x9))*x6 
+        dx2 = np.cos(x8)*np.sin(x9)*x4+(np.sin(x7)*np.sin(x8)*np.sin(x9)+np.cos(x7)*np.cos(x9))*x5+(np.cos(x7)*np.sin(x8)*np.sin(x9)-np.sin(x7)*np.cos(x9))*x6  
+        dx3 = np.sin(x8)*x4-np.sin(x7)*np.cos(x8)*x5-np.cos(x7)*np.cos(x8)*x6 
+        dx4 = x12*x5-x11*x6-g*np.sin(x8)
+        dx5 = x10*x6-x12*x4+g*np.cos(x8)*np.sin(x7)
+        dx6 = x11*x4-x10*x5+g*np.cos(x8)*np.cos(x7)-F/m 
+        dx7 = x10+np.sin(x7)*np.tan(x8)*x11+np.cos(x7)*np.tan(x8)*x12
+        dx8 = np.cos(x7)*x11-np.sin(x7)*x12 
+        dx9 = np.sin(x7)/np.cos(x8)*x11+np.cos(x7)/np.cos(x8)*x12 
+        dx10 = (Jy-Jz)/Jx*x11*x12+1/Jx*tau_phi
+        dx11 = (Jz-Jx)/Jy*x10*x12+1/Jy*tau_theta 
+        dx12 = (Jx-Jy)/Jz*x10*x11+1/Jz*tau_psi
+        du1 = 0 
+
+        return [dx1,dx2,dx3,dx4,dx5,dx6,dx7,dx8,dx9,dx10,dx11,dx12,du1]
+
+    def TC_simulate(self, mode, initialSet, time_horizon, time_step, map=None):
+        y0 = initialSet 
+        t = np.round(np.arange(0.0, time_horizon+time_step/2, time_step), 8)
+        trace = odeint(func = self.dynamics, y0 = y0, t = t)
+        t = t.reshape((-1,1))
+        trace = np.hstack((t, trace))
+        return trace
+
diff --git a/demo/dryvr_demo/origin_agent.py b/demo/dryvr_demo/origin_agent.py
index a6669360..2cf5e525 100644
--- a/demo/dryvr_demo/origin_agent.py
+++ b/demo/dryvr_demo/origin_agent.py
@@ -7,7 +7,6 @@
 from verse.agents import BaseAgent
 from verse.map import LaneMap
 
-
 class vanderpol_agent(BaseAgent):
     def __init__(self, id, code=None, file_name=None):
         # Calling the constructor of tha base class
diff --git a/demo/dryvr_demo/quadrotor_demo.py b/demo/dryvr_demo/quadrotor_demo.py
new file mode 100644
index 00000000..24a33d1f
--- /dev/null
+++ b/demo/dryvr_demo/quadrotor_demo.py
@@ -0,0 +1,39 @@
+from dryvr_agent import QuadrotorAgent 
+from verse import Scenario, ScenarioConfig
+from verse.plotter.plotter2D import * 
+
+import plotly.graph_objects as go 
+from enum import Enum, auto 
+
+class AgentMode(Enum):
+    Default = auto()
+
+if __name__ == "__main__":
+    scenario = Scenario(ScenarioConfig(parallel=False))
+
+    quad = QuadrotorAgent('quad')
+    scenario.add_agent(quad)
+    # The initial position of the quadrotor is uncertain in 
+    # all directions within [−0.4, 0.4] [m] and also the velocity 
+    # is uncertain within [−0.4, 0.4] [m/s] for all directions
+    
+    # The inertial (north) position x1, the inertial (east) position x2, 
+    # the altitude x3, the longitudinal velocity x4, 
+    # the lateral velocity x5, the vertical velocity x6, 
+    # the roll angle x7, the pitch angle x8, 
+    # the yaw angle x9, the roll rate x10, 
+    # the pitch rate x11, and the yaw rate x12.
+    scenario.set_init(
+        [
+            [[-0.4,-0.4,-0.4,-0.4,-0.4,-0.4, 0, 0, 0, 0, 0, 0, 0.98],
+             [ 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0, 0, 0, 0, 0, 0, 1.02]]
+        ],
+        [
+            (AgentMode.Default, )
+        ]
+    )
+    traces = scenario.verify(5, 0.01)
+
+    fig = go.Figure() 
+    fig = reachtube_tree(traces, None, fig, 0, 3, [1,3], "lines", "trace")
+    fig.show()
\ No newline at end of file