(WIP) feat: Quadcopter model where each arm can tilt by an angle alpha

docs/envs.rst

@@ -12,23 +12,30 @@ BaseDroneEnv
    :undoc-members:
    :show-inheritance:
 
-PyBulletDroneEnv
-----------------
-.. autoclass:: jdrones.envs.PyBulletDroneEnv
+QuadPyBulletDroneEnv
+--------------------
+.. autoclass:: jdrones.envs.QuadPyBulletDroneEnv
+   :members:
+   :undoc-members:
+   :show-inheritance:
+
+QuadNonlinearDynamicModelDroneEnv
+---------------------------------
+.. autoclass:: jdrones.envs.QuadNonlinearDynamicModelDroneEnv
    :members:
    :undoc-members:
    :show-inheritance:
 
-NonlinearDynamicModelDroneEnv
------------------------------
-.. autoclass:: jdrones.envs.NonlinearDynamicModelDroneEnv
+QuadLinearDynamicModelDroneEnv
+------------------------------
+.. autoclass:: jdrones.envs.QuadLinearDynamicModelDroneEnv
    :members:
    :undoc-members:
    :show-inheritance:
 
-LinearDynamicModelDroneEnv
---------------------------
-.. autoclass:: jdrones.envs.LinearDynamicModelDroneEnv
+XWingNonLinearDynamicModelDroneEnv
+----------------------------------
+.. autoclass:: jdrones.envs.XWingNonlinearDynamicModelDroneEnv
    :members:
    :undoc-members:
    :show-inheritance:
@@ -73,3 +80,12 @@ PolyPositionDroneEnv
    :members:
    :undoc-members:
    :show-inheritance:
+
+Models
+======
+
+
+.. automodule:: jdrones.envs.dronemodels
+   :members:
+   :undoc-members:
+   :show-inheritance:

docs/examples/linearise_nonlinear_model.ipynb

@@ -52,28 +52,29 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 3,
    "id": "aba89791",
    "metadata": {},
    "outputs": [],
    "source": [
-    "env = gymnasium.make('NonLinearDynamicModelDroneEnv-v0')"
+    "env = gymnasium.make('RotatingNonlinearDynamicModelDroneEnv-v0')"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 4,
    "id": "34ed851b",
    "metadata": {},
    "outputs": [],
    "source": [
-    "utrim = np.ones(4)*np.sqrt((env.model.mass*env.model.g)/(4*env.model.k_T))\n",
+    "utrim = np.concatenate([np.ones(4)*np.sqrt((env.model.mass*env.model.g)/(4*env.model.k_T)),[0]])\n",
+    "# utrim = np.ones(4)*np.sqrt((env.model.mass*env.model.g)/(4*env.model.k_T))\n",
     "xtrim = np.zeros(12)"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 5,
    "id": "21f65a03",
    "metadata": {},
    "outputs": [],
@@ -124,7 +125,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 9,
+   "execution_count": 6,
    "id": "520c9f9e",
    "metadata": {},
    "outputs": [
@@ -135,14 +136,14 @@
       "(0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
       "(0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
       "(0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 9.81, 0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0, -0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.81, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
       "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0)\n",
       "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0)\n",
       "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)\n",
-      "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
       "(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n"
      ]
     }
@@ -154,33 +155,41 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 10,
+   "execution_count": 7,
    "id": "6d7a0025",
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "(0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0)\n",
-      "(0.84, 0.84, 0.84, 0.84)\n",
-      "(0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, 0.0, 0.0, 0.0)\n",
-      "(0.0, -1.17, 0.0, 1.17)\n",
-      "(-1.17, 0.0, 1.17, 0.0)\n",
-      "(5.86, -5.86, 5.86, -5.86)\n"
+      "(0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.84, 0.84, 0.84, 0.84, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, 0.0, 0.0, 0.0, 0.0)\n",
+      "(0.0, -1.17, 0.0, 1.17, 0.0)\n",
+      "(1.17, 0.0, -1.17, 0.0, 0.0)\n",
+      "(-117.19, 117.19, -117.19, 117.19, 13.73)\n"
      ]
     }
    ],
    "source": [
     "for row in conMatrix:\n",
     "    print(tuple(map(functools.partial(round, ndigits=2), row)))"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "f77fd3a8",
+   "metadata": {},
+   "outputs": [],
+   "source": []
   }
  ],
  "metadata": {

docs/examples/mpc_drone.ipynb

@@ -102,7 +102,7 @@
    "outputs": [],
    "source": [
     "def prediction(u, x, dt=mpc_dt):\n",
-    "    env = gymnasium.make(\"LinearDynamicModelDroneEnv-v0\", dt=dt, initial_state=x)\n",
+    "    env = gymnasium.make(\"QuadLinearDynamicModelDroneEnv-v0\", dt=dt, initial_state=x)\n",
     "    y = collections.deque()\n",
     "    obs, _ = env.reset()\n",
     "    assert np.allclose(obs.pos, x.pos)\n",

docs/examples/visual_validations.ipynb

@@ -88,7 +88,7 @@
     "\n",
     "\n",
     "l_env = gymnasium.make(\n",
-    "    \"LinearDynamicModelDroneEnv-v0\", dt=dt, initial_state=initial_state\n",
+    "    \"QuadLinearDynamicModelDroneEnv-v0\", dt=dt, initial_state=initial_state\n",
     ")\n",
     "l_env = LinEnvUpdateWrapper(l_env)"
    ]

src/jdrones/__init__.py

@@ -3,14 +3,18 @@
 from gymnasium.envs.registration import register
 
 
-register("PyBulletDroneEnv-v0", entry_point="jdrones.envs:PyBulletDroneEnv")
+register("QuadPyBulletDroneEnv-v0", entry_point="jdrones.envs:QuadPyBulletDroneEnv")
 register(
-    "NonLinearDynamicModelDroneEnv-v0",
-    entry_point="jdrones.envs:NonlinearDynamicModelDroneEnv",
+    "QuadNonLinearDynamicModelDroneEnv-v0",
+    entry_point="jdrones.envs:QuadNonlinearDynamicModelDroneEnv",
 )
 register(
-    "LinearDynamicModelDroneEnv-v0",
-    entry_point="jdrones.envs:LinearDynamicModelDroneEnv",
+    "QuadLinearDynamicModelDroneEnv-v0",
+    entry_point="jdrones.envs:QuadLinearDynamicModelDroneEnv",
+)
+register(
+    "XWingNonlinearDynamicModelDroneEnv-v0",
+    entry_point="jdrones.envs:XWingNonlinearDynamicModelDroneEnv",
 )
 register(
     "LQRDroneEnv-v0",

src/jdrones/envs/__init__.py

@@ -1,21 +1,23 @@
 #  Copyright 2023 Jan-Hendrik Ewers
 #  SPDX-License-Identifier: GPL-3.0-only
-from .base import LinearDynamicModelDroneEnv
-from .base import NonlinearDynamicModelDroneEnv
-from .base import PyBulletDroneEnv
 from .base.basecontrolledenv import BaseControlledEnv
 from .dronemodels import DronePlus
 from .lqr import LQRDroneEnv
 from .position import LQRPositionDroneEnv
 from .position import PolyPositionDroneEnv
+from .quad import QuadLinearDynamicModelDroneEnv
+from .quad import QuadNonlinearDynamicModelDroneEnv
+from .quad import QuadPyBulletDroneEnv
+from .x_wing import XWingNonlinearDynamicModelDroneEnv
 
 __all__ = [
-    "PyBulletDroneEnv",
-    "NonlinearDynamicModelDroneEnv",
-    "LinearDynamicModelDroneEnv",
+    "QuadPyBulletDroneEnv",
+    "QuadNonlinearDynamicModelDroneEnv",
+    "QuadLinearDynamicModelDroneEnv",
     "LQRDroneEnv",
     "LQRPositionDroneEnv",
     "DronePlus",
     "PolyPositionDroneEnv",
     "BaseControlledEnv",
+    "XWingNonlinearDynamicModelDroneEnv",
 ]

src/jdrones/envs/base/__init__.py

@@ -1,13 +1,7 @@
 #  Copyright 2023 Jan-Hendrik Ewers
 #  SPDX-License-Identifier: GPL-3.0-only
 from .basecontrolledenv import BaseControlledEnv
-from .lineardronenev import LinearDynamicModelDroneEnv
-from .nonlineardronenev import NonlinearDynamicModelDroneEnv
-from .pbdronenev import PyBulletDroneEnv
 
 __all__ = [
     "BaseControlledEnv",
-    "PyBulletDroneEnv",
-    "LinearDynamicModelDroneEnv",
-    "NonlinearDynamicModelDroneEnv",
 ]

src/jdrones/envs/dronemodels.py

@@ -4,9 +4,43 @@
 
 import numpy as np
 from jdrones.data_models import URDFModel
+from jdrones.types import MAT4X4
 
 
-def droneplus_mixing_matrix(*, length, k_Q, k_T):
+def droneplus_mixing_matrix(*, length: float, k_Q: float, k_T: float) -> MAT4X4:
+    """
+    .. math::
+        \\vec M
+        = \\begin{bmatrix}
+            \\vec \\Gamma \\\\
+            T
+        \\end{bmatrix}
+        =
+        \\begin{bmatrix}
+            \\Gamma_\\phi\\\\\\Gamma_\\theta\\\\\\Gamma_\\psi\\\\T
+        \\end{bmatrix}
+        = \\begin{bmatrix}
+        0& -l k_T& 0& l k_T \\\\
+        -l k_T& 0& l k_T& 0\\\\
+        k_Q&-k_Q& k_Q& -k_Q \\\\
+        k_T & k_T & k_T & k_T
+        \\end{bmatrix}
+        \\begin{bmatrix}
+            P_1\\\\P_2\\\\P_3\\\\P_4
+        \\end{bmatrix}
+
+    Parameters
+    ----------
+    length : float
+    k_Q : float
+    k_T : float
+
+    Returns
+    -------
+    jdrones.types.MAT4X4
+        Mixing matrix
+    """
+
     return np.array(
         [
             [0, -k_T * length, 0, k_T * length],

src/jdrones/envs/lqr.py

@@ -10,9 +10,9 @@
 from jdrones.data_models import State
 from jdrones.data_models import URDFModel
 from jdrones.envs.base import BaseControlledEnv
-from jdrones.envs.base import LinearDynamicModelDroneEnv
-from jdrones.envs.base import NonlinearDynamicModelDroneEnv
 from jdrones.envs.dronemodels import DronePlus
+from jdrones.envs.quad import QuadLinearDynamicModelDroneEnv
+from jdrones.envs.quad import QuadNonlinearDynamicModelDroneEnv
 from jdrones.types import LinearXAction
 
 
@@ -22,12 +22,12 @@ def __init__(
         model: URDFModel = DronePlus,
         initial_state: State = None,
         dt: float = 1 / 240,
-        env: NonlinearDynamicModelDroneEnv = None,
+        env: QuadNonlinearDynamicModelDroneEnv = None,
         Q=None,
         R=None,
     ):
         if env is None:
-            env = NonlinearDynamicModelDroneEnv(
+            env = QuadNonlinearDynamicModelDroneEnv(
                 model=model, initial_state=initial_state, dt=dt
             )
 
@@ -66,7 +66,7 @@ def __init__(
         super().__init__(env, dt)
 
     def _init_controllers(self) -> dict[str, Controller]:
-        A, B, _ = LinearDynamicModelDroneEnv.get_matrices(self.env.model)
+        A, B, _ = QuadLinearDynamicModelDroneEnv.get_matrices(self.env.model)
         return dict(lqr=LQR(A, B, self.Q, self.R))
 
     def reset(

src/jdrones/envs/quad/__init__.py

@@ -0,0 +1,5 @@
+#  Copyright 2023 Jan-Hendrik Ewers
+#  SPDX-License-Identifier: GPL-3.0-only
+from .lineardronenev import QuadLinearDynamicModelDroneEnv
+from .nonlineardronenev import QuadNonlinearDynamicModelDroneEnv
+from .pbdronenev import QuadPyBulletDroneEnv

src/jdrones/envs/base/lineardronenev.py → src/jdrones/envs/quad/lineardronenev.py

@@ -13,7 +13,7 @@
 from jdrones.types import PropellerAction
 
 
-class LinearDynamicModelDroneEnv(BaseDroneEnv):
+class QuadLinearDynamicModelDroneEnv(BaseDroneEnv):
     def __init__(
         self,
         model: URDFModel = DronePlus,
@@ -119,36 +119,23 @@ def step(self, action: PropellerAction) -> Tuple[State, float, bool, bool, dict]
                 0 & 0 & 0 & 0\\\\
                 0 & 0 & 0 & 0\\\\
                 0 & 0 & 0 & 0\\\\
-                0 & 0 & 0 & -1 / m\\\\
+                0 & 0 & 0 & 1 / m\\\\
                 0 & 0 & 0 & 0\\\\
                 0 & 0 & 0 & 0\\\\
                 0 & 0 & 0 & 0\\\\
                 1/Ix & 0 & 0 & 0\\\\
                 0 & 1/Iy & 0 & 0\\\\
                 0 & & 1 / Iz & 0
             \\end{bmatrix}
-            \\begin{bmatrix}
-                \\tau_\\phi\\\\\\tau_\\theta\\\\\\tau_\\psi\\\\T
-            \\end{bmatrix}
+            \\vec M
             +
             \\begin{bmatrix}
                 0\\\\0\\\\0\\\\0\\\\0\\\\g\\\\0\\\\0\\\\0\\\\0\\\\0\\\\0
             \\end{bmatrix}
 
-        .. math::
-
-            \\begin{bmatrix}
-                \\tau_\\phi\\\\\\tau_\\theta\\\\\\tau_\\psi\\\\T
-            \\end{bmatrix}
-             &= \\begin{bmatrix}
-            0& -l k_T& 0& l k_T \\\\
-            l k_T& 0& -l k_T& 0\\\\
-            -k_Q&k_Q& -k_Q& k_Q \\\\
-            k_T & k_T & k_T & k_T
-            \\end{bmatrix}
-            \\begin{bmatrix}
-                P_1\\\\P_2\\\\P_3\\\\P_4
-            \\end{bmatrix}
+        .. seealso::
+            :math:`\\vec M = [\\vec \\Gamma, T]^T` is defined in
+            :func:`jdrones.envs.dronemodels.droneplus_mixing_matrix`
 
         Parameters
         ----------