From 2d2148e5f0627754c77b2952476e13b18dca0867 Mon Sep 17 00:00:00 2001 From: iwishiwasaneagle Date: Mon, 6 Mar 2023 17:06:36 +0000 Subject: [PATCH 1/8] feat: Quadcopter model where each arm can tilt by an angle alpha --- src/jdrones/__init__.py | 4 + src/jdrones/envs/__init__.py | 2 + src/jdrones/envs/rotating.py | 176 +++++++++++++++++++++++++++++++++++ src/jdrones/types.py | 2 + 4 files changed, 184 insertions(+) create mode 100644 src/jdrones/envs/rotating.py diff --git a/src/jdrones/__init__.py b/src/jdrones/__init__.py index cd1cde6..79b8282 100644 --- a/src/jdrones/__init__.py +++ b/src/jdrones/__init__.py @@ -12,6 +12,10 @@ "LinearDynamicModelDroneEnv-v0", entry_point="jdrones.envs:LinearDynamicModelDroneEnv", ) +register( + "RotatingNonlinearDynamicModelDroneEnv-v0", + entry_point="jdrones.envs:RotatingNonlinearDynamicModelDroneEnv", +) register( "LQRDroneEnv-v0", entry_point="jdrones.envs:LQRDroneEnv", diff --git a/src/jdrones/envs/__init__.py b/src/jdrones/envs/__init__.py index ecbe74d..adf0f51 100644 --- a/src/jdrones/envs/__init__.py +++ b/src/jdrones/envs/__init__.py @@ -8,6 +8,7 @@ from .lqr import LQRDroneEnv from .position import LQRPositionDroneEnv from .position import PolyPositionDroneEnv +from .rotating import RotatingNonlinearDynamicModelDroneEnv __all__ = [ "PyBulletDroneEnv", @@ -18,4 +19,5 @@ "DronePlus", "PolyPositionDroneEnv", "BaseControlledEnv", + "RotatingNonlinearDynamicModelDroneEnv", ] diff --git a/src/jdrones/envs/rotating.py b/src/jdrones/envs/rotating.py new file mode 100644 index 0000000..10648f1 --- /dev/null +++ b/src/jdrones/envs/rotating.py @@ -0,0 +1,176 @@ +# Copyright 2023 Jan-Hendrik Ewers +# SPDX-License-Identifier: GPL-3.0-only +from typing import Tuple + +import numpy as np +from gymnasium import spaces +from jdrones.data_models import State +from jdrones.data_models import URDFModel +from jdrones.envs.base.basedronenev import BaseDroneEnv +from jdrones.transforms import euler_to_quat +from jdrones.transforms import euler_to_rotmat +from jdrones.types import PropellerAction +from jdrones.types import VEC5 + + +class RotatingNonlinearDynamicModelDroneEnv(BaseDroneEnv): + @property + def action_space(self): + act_bounds = np.array( + [ + (0.0, 1e6), # R1 + (0.0, 1e6), # R2 + (0.0, 1e6), # R3 + (0.0, 1e6), # R4 + (-np.pi, np.pi), # Alpha + ] + ) + return spaces.Box( + low=act_bounds[:, 0], + high=act_bounds[:, 1], + dtype=float, + ) + + @staticmethod + def calc_dstate(action: VEC5, state: State, model: URDFModel): + Inertias = np.diag(model.I) + m = model.mass + g = model.g + length = model.l + P = action[:4] + alpha = action[4] + + T = model.k_T * np.square(P) + + P1, P2, P3, P4 = P + T1, T2, T3, T4 = T + + unit_z = np.array([0, 0, 1]).reshape((-1, 1)) + + R_W_B = euler_to_rotmat(state.rpy) + R_B_W = np.linalg.inv(R_W_B) + + dstate = np.concatenate( + [ + state.vel, + (0, 0, 0, 0), + state.ang_vel, + ( + -m * g * unit_z.T + + ( + R_B_W + @ [ + (T4 - T2) * np.sin(alpha), + (T3 - T1) * np.sin(alpha), + T.sum() * np.cos(alpha), + ] + ).T + ).flatten() + / m, + np.linalg.solve( + Inertias, + ( + R_B_W + @ [ + length * (T4 - T2) * np.cos(alpha), + length * (T3 - T1) * np.cos(alpha), + length * T.sum() * np.sin(alpha) + + model.k_Q + + (-P1 * P1 + P2 * P2 - P3 * P3 + P4 * P4) * np.cos(alpha), + ] + ), + ), + (0, 0, 0, 0), + ] + ) + return dstate + + def step(self, action: PropellerAction) -> Tuple[State, float, bool, bool, dict]: + # Get state + dstate = self.calc_dstate(action, self.state, self.model) + + # Update step + self.state += self.dt * dstate + + # Update derived state items + self.state.prop_omega = action[:4] + self.state.quat = euler_to_quat(self.state.rpy) + + # Return + return self.state, 0, False, False, self.info + + +if __name__ == "__main__": + from collections import deque + + import matplotlib.pyplot as plt + import pandas as pd + import seaborn as sns + from jdrones.data_models import States + from tqdm.auto import trange + from loguru import logger + + def _simulate_env(env, T, dt): + dq = deque() + env.reset() + trim = np.sqrt(1.4 * 9.81 / (0.1 * 4)) + setpoint = np.array([*(trim,) * 4, 1e-6]) + for _ in trange(int(T / dt)): + obs, *_ = env.step(setpoint) + dq.append(np.copy(obs)) + return dq + + T = 5 + dt = 1 / 240 + + initial_state = State() + initial_state.pos = (0, 0, 10) + initial_state.rpy = (0, 0, 0) + + nl_env = RotatingNonlinearDynamicModelDroneEnv( + initial_state=initial_state, + dt=dt, + ) + logger.debug("Simulating envs") + df = pd.concat( + [ + States(_simulate_env(f, T, dt)).to_df(tag=type(f).__name__, N=200, dt=dt) + for f in [nl_env] + ] + ).reset_index() + logger.debug("Plotting grouped states") + print(df.head()) + + fig, ax = plt.subplots(4, figsize=(10, 8)) + ax = ax.flatten() + for i, vars in enumerate( + ["'x','y','z'", "'phi','theta','psi'", "'vx','vy','vz'", "'P0','P1','P2','P3'"] + ): + sns.lineplot( + data=df.query(f"variable in ({vars})"), + x="t", + y="value", + hue="variable", + style="tag", + ax=ax[i], + ) + ax[i].legend() + fig.tight_layout() + plt.show() + + logger.debug("Plotting individual states") + fig, ax = plt.subplots(9, figsize=(14, 10)) + ax = ax.flatten() + for i, var in enumerate(("x", "y", "z", "phi", "theta", "psi", "vx", "vy", "vz")): + sns.lineplot( + data=df.query(f"variable in ('{var}')"), + x="t", + y="value", + hue="variable", + style="tag", + ax=ax[i], + legend=False, + ) + ax[i].set_ylabel(var) + fig.tight_layout() + plt.show() diff --git a/src/jdrones/types.py b/src/jdrones/types.py index 594100c..f2c37e2 100644 --- a/src/jdrones/types.py +++ b/src/jdrones/types.py @@ -6,6 +6,8 @@ VEC4 = tuple[float, float, float, float] """:math:`(a,b,c,d)` vector""" MAT3X3 = tuple[VEC3, VEC3, VEC3] +VEC5 = tuple[float, float, float, float, float] +""":math:`(a,b,c,d,e)` vector""" """:math:`3 \\times 3` matrix""" MAT4X3 = tuple[VEC4, VEC4, VEC4, VEC4] """:math:`4 \\times 4` matrix""" From 92f7704347539ac1e5b98d043bb234bfff0343c4 Mon Sep 17 00:00:00 2001 From: iwishiwasaneagle Date: Mon, 6 Mar 2023 17:43:52 +0000 Subject: [PATCH 2/8] refactor: Rename normal drone to "quad" and introduce "x-wing" --- docs/envs.rst | 25 ++-- docs/examples/linearise_nonlinear_model.ipynb | 57 +++++---- docs/examples/mpc_drone.ipynb | 2 +- docs/examples/visual_validations.ipynb | 2 +- src/jdrones/__init__.py | 14 +-- src/jdrones/envs/__init__.py | 16 +-- src/jdrones/envs/base/__init__.py | 6 - src/jdrones/envs/lqr.py | 10 +- src/jdrones/envs/quad/__init__.py | 5 + src/jdrones/envs/{base => quad}/__main__.py | 0 .../envs/{base => quad}/lineardronenev.py | 2 +- .../envs/{base => quad}/nonlineardronenev.py | 2 +- src/jdrones/envs/{base => quad}/pbdronenev.py | 2 +- src/jdrones/envs/x_wing/__init__.py | 3 + .../nonlineardroneenv.py} | 33 +++-- tests/conftest.py | 26 ++-- tests/envs/base/test_linear_drone.py | 105 ---------------- tests/envs/base/test_nonlinear_drone.py | 115 ------------------ tests/envs/{base => }/conftest.py | 0 tests/envs/quad/test_linear_drone.py | 105 ++++++++++++++++ tests/envs/quad/test_nonlinear_drone.py | 115 ++++++++++++++++++ .../{base => quad}/test_pybullet3_drone.py | 98 +++++++-------- tests/test_gym_api.py | 24 ++-- 23 files changed, 397 insertions(+), 370 deletions(-) create mode 100644 src/jdrones/envs/quad/__init__.py rename src/jdrones/envs/{base => quad}/__main__.py (100%) rename src/jdrones/envs/{base => quad}/lineardronenev.py (99%) rename src/jdrones/envs/{base => quad}/nonlineardronenev.py (97%) rename src/jdrones/envs/{base => quad}/pbdronenev.py (99%) create mode 100644 src/jdrones/envs/x_wing/__init__.py rename src/jdrones/envs/{rotating.py => x_wing/nonlineardroneenv.py} (83%) delete mode 100644 tests/envs/base/test_linear_drone.py delete mode 100644 tests/envs/base/test_nonlinear_drone.py rename tests/envs/{base => }/conftest.py (100%) create mode 100644 tests/envs/quad/test_linear_drone.py create mode 100644 tests/envs/quad/test_nonlinear_drone.py rename tests/envs/{base => quad}/test_pybullet3_drone.py (62%) diff --git a/docs/envs.rst b/docs/envs.rst index e275b6c..2c464fe 100644 --- a/docs/envs.rst +++ b/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: diff --git a/docs/examples/linearise_nonlinear_model.ipynb b/docs/examples/linearise_nonlinear_model.ipynb index 8c23f1a..4830875 100644 --- a/docs/examples/linearise_nonlinear_model.ipynb +++ b/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,7 +155,7 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 7, "id": "6d7a0025", "metadata": {}, "outputs": [ @@ -162,18 +163,18 @@ "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" ] } ], @@ -181,6 +182,14 @@ "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": { diff --git a/docs/examples/mpc_drone.ipynb b/docs/examples/mpc_drone.ipynb index f680d6e..b9aab5c 100644 --- a/docs/examples/mpc_drone.ipynb +++ b/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", diff --git a/docs/examples/visual_validations.ipynb b/docs/examples/visual_validations.ipynb index fd5f378..43e8c8e 100644 --- a/docs/examples/visual_validations.ipynb +++ b/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)" ] diff --git a/src/jdrones/__init__.py b/src/jdrones/__init__.py index 79b8282..e4f618b 100644 --- a/src/jdrones/__init__.py +++ b/src/jdrones/__init__.py @@ -3,18 +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( - "RotatingNonlinearDynamicModelDroneEnv-v0", - entry_point="jdrones.envs:RotatingNonlinearDynamicModelDroneEnv", + "XWingNonlinearDynamicModelDroneEnv-v0", + entry_point="jdrones.envs:XWingNonlinearDynamicModelDroneEnv", ) register( "LQRDroneEnv-v0", diff --git a/src/jdrones/envs/__init__.py b/src/jdrones/envs/__init__.py index adf0f51..7932fd9 100644 --- a/src/jdrones/envs/__init__.py +++ b/src/jdrones/envs/__init__.py @@ -1,23 +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 .rotating import RotatingNonlinearDynamicModelDroneEnv +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", - "RotatingNonlinearDynamicModelDroneEnv", + "XWingNonlinearDynamicModelDroneEnv", ] diff --git a/src/jdrones/envs/base/__init__.py b/src/jdrones/envs/base/__init__.py index e29149f..fe46105 100644 --- a/src/jdrones/envs/base/__init__.py +++ b/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", ] diff --git a/src/jdrones/envs/lqr.py b/src/jdrones/envs/lqr.py index f3cd9a3..f823bec 100644 --- a/src/jdrones/envs/lqr.py +++ b/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( diff --git a/src/jdrones/envs/quad/__init__.py b/src/jdrones/envs/quad/__init__.py new file mode 100644 index 0000000..538c781 --- /dev/null +++ b/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 diff --git a/src/jdrones/envs/base/__main__.py b/src/jdrones/envs/quad/__main__.py similarity index 100% rename from src/jdrones/envs/base/__main__.py rename to src/jdrones/envs/quad/__main__.py diff --git a/src/jdrones/envs/base/lineardronenev.py b/src/jdrones/envs/quad/lineardronenev.py similarity index 99% rename from src/jdrones/envs/base/lineardronenev.py rename to src/jdrones/envs/quad/lineardronenev.py index 565ec8f..e61ad48 100644 --- a/src/jdrones/envs/base/lineardronenev.py +++ b/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, diff --git a/src/jdrones/envs/base/nonlineardronenev.py b/src/jdrones/envs/quad/nonlineardronenev.py similarity index 97% rename from src/jdrones/envs/base/nonlineardronenev.py rename to src/jdrones/envs/quad/nonlineardronenev.py index 704e4eb..987cdea 100644 --- a/src/jdrones/envs/base/nonlineardronenev.py +++ b/src/jdrones/envs/quad/nonlineardronenev.py @@ -11,7 +11,7 @@ from jdrones.types import PropellerAction -class NonlinearDynamicModelDroneEnv(BaseDroneEnv): +class QuadNonlinearDynamicModelDroneEnv(BaseDroneEnv): @staticmethod def calc_dstate(action: PropellerAction, state: State, model: URDFModel): Inertias = np.diag(model.I) diff --git a/src/jdrones/envs/base/pbdronenev.py b/src/jdrones/envs/quad/pbdronenev.py similarity index 99% rename from src/jdrones/envs/base/pbdronenev.py rename to src/jdrones/envs/quad/pbdronenev.py index 283e7a9..95c6849 100644 --- a/src/jdrones/envs/base/pbdronenev.py +++ b/src/jdrones/envs/quad/pbdronenev.py @@ -25,7 +25,7 @@ from jdrones.types import VEC4 -class PyBulletDroneEnv(BaseDroneEnv): +class QuadPyBulletDroneEnv(BaseDroneEnv): """ Base drone environment. Handles pybullet loading, and application of forces. Generalizes the physics to allow other models to be used. diff --git a/src/jdrones/envs/x_wing/__init__.py b/src/jdrones/envs/x_wing/__init__.py new file mode 100644 index 0000000..dc7bd73 --- /dev/null +++ b/src/jdrones/envs/x_wing/__init__.py @@ -0,0 +1,3 @@ +# Copyright 2023 Jan-Hendrik Ewers +# SPDX-License-Identifier: GPL-3.0-only +from .nonlineardroneenv import XWingNonlinearDynamicModelDroneEnv diff --git a/src/jdrones/envs/rotating.py b/src/jdrones/envs/x_wing/nonlineardroneenv.py similarity index 83% rename from src/jdrones/envs/rotating.py rename to src/jdrones/envs/x_wing/nonlineardroneenv.py index 10648f1..efcc95b 100644 --- a/src/jdrones/envs/rotating.py +++ b/src/jdrones/envs/x_wing/nonlineardroneenv.py @@ -13,7 +13,7 @@ from jdrones.types import VEC5 -class RotatingNonlinearDynamicModelDroneEnv(BaseDroneEnv): +class XWingNonlinearDynamicModelDroneEnv(BaseDroneEnv): @property def action_space(self): act_bounds = np.array( @@ -37,18 +37,14 @@ def calc_dstate(action: VEC5, state: State, model: URDFModel): m = model.mass g = model.g length = model.l - P = action[:4] alpha = action[4] - T = model.k_T * np.square(P) - - P1, P2, P3, P4 = P - T1, T2, T3, T4 = T + T1, T2, T3, T4 = model.k_T * np.square(action[:4]) + u_star = model.rpm2rpyT(np.square(action[:4])) unit_z = np.array([0, 0, 1]).reshape((-1, 1)) - R_W_B = euler_to_rotmat(state.rpy) - R_B_W = np.linalg.inv(R_W_B) + R_W_B = np.array(euler_to_rotmat(state.rpy)) dstate = np.concatenate( [ @@ -58,11 +54,11 @@ def calc_dstate(action: VEC5, state: State, model: URDFModel): ( -m * g * unit_z.T + ( - R_B_W + R_W_B @ [ - (T4 - T2) * np.sin(alpha), - (T3 - T1) * np.sin(alpha), - T.sum() * np.cos(alpha), + T4 * np.sin(alpha) + T2 * np.sin(-alpha), + T1 * np.sin(alpha) + T4 * np.sin(-alpha), + u_star[3] * np.cos(alpha), ] ).T ).flatten() @@ -70,13 +66,12 @@ def calc_dstate(action: VEC5, state: State, model: URDFModel): np.linalg.solve( Inertias, ( - R_B_W + R_W_B @ [ - length * (T4 - T2) * np.cos(alpha), - length * (T3 - T1) * np.cos(alpha), - length * T.sum() * np.sin(alpha) - + model.k_Q - + (-P1 * P1 + P2 * P2 - P3 * P3 + P4 * P4) * np.cos(alpha), + u_star[0] * np.cos(alpha), + u_star[1] * np.cos(alpha), + length * u_star[3] * np.sin(alpha) + + u_star[2] * np.cos(alpha), ] ), ), @@ -127,7 +122,7 @@ def _simulate_env(env, T, dt): initial_state.pos = (0, 0, 10) initial_state.rpy = (0, 0, 0) - nl_env = RotatingNonlinearDynamicModelDroneEnv( + nl_env = XWingNonlinearDynamicModelDroneEnv( initial_state=initial_state, dt=dt, ) diff --git a/tests/conftest.py b/tests/conftest.py index a56a40f..03ac9d4 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -8,12 +8,13 @@ from jdrones.data_models import SimulationType from jdrones.data_models import State from jdrones.data_models import URDFModel -from jdrones.envs import LinearDynamicModelDroneEnv from jdrones.envs import LQRDroneEnv from jdrones.envs import LQRPositionDroneEnv -from jdrones.envs import NonlinearDynamicModelDroneEnv from jdrones.envs import PolyPositionDroneEnv -from jdrones.envs import PyBulletDroneEnv +from jdrones.envs import QuadLinearDynamicModelDroneEnv +from jdrones.envs import QuadNonlinearDynamicModelDroneEnv +from jdrones.envs import QuadPyBulletDroneEnv +from jdrones.envs import XWingNonlinearDynamicModelDroneEnv from jdrones.envs.dronemodels import droneplus_mixing_matrix from jdrones.envs.position import BasePositionDroneEnv from jdrones.transforms import euler_to_quat @@ -185,22 +186,29 @@ def env_default_kwargs(urdfmodel, dt, state): @pytest.fixture -def pbdroneenv(env_default_kwargs, simulation_type): - d = PyBulletDroneEnv(**env_default_kwargs, simulation_type=simulation_type) +def quadpbdroneenv(env_default_kwargs, simulation_type): + d = QuadPyBulletDroneEnv(**env_default_kwargs, simulation_type=simulation_type) yield d d.close() @pytest.fixture -def nonlineardroneenv(env_default_kwargs): - d = NonlinearDynamicModelDroneEnv(**env_default_kwargs) +def quadnonlineardroneenv(env_default_kwargs): + d = QuadNonlinearDynamicModelDroneEnv(**env_default_kwargs) yield d d.close() @pytest.fixture -def lineardroneenv(env_default_kwargs): - d = LinearDynamicModelDroneEnv(**env_default_kwargs) +def quadlineardroneenv(env_default_kwargs): + d = QuadLinearDynamicModelDroneEnv(**env_default_kwargs) + yield d + d.close() + + +@pytest.fixture +def xwingnonlineardroneenv(env_default_kwargs): + d = XWingNonlinearDynamicModelDroneEnv(**env_default_kwargs) yield d d.close() diff --git a/tests/envs/base/test_linear_drone.py b/tests/envs/base/test_linear_drone.py deleted file mode 100644 index c2f9790..0000000 --- a/tests/envs/base/test_linear_drone.py +++ /dev/null @@ -1,105 +0,0 @@ -# Copyright 2023 Jan-Hendrik Ewers -# SPDX-License-Identifier: GPL-3.0-only -import numpy as np -import pytest -from envs.base.conftest import INPUT_TO_ROT -from envs.base.conftest import LARGE_INPUT -from envs.base.conftest import LOW_INPUT -from envs.base.conftest import PITCH_INPUT -from envs.base.conftest import POSITION_FROM_VELOCITY_1 -from envs.base.conftest import POSITION_FROM_VELOCITY_2 -from envs.base.conftest import ROLL_INPUT -from envs.base.conftest import RPY_FROM_ANG_VEL -from envs.base.conftest import VELOCITY_FROM_ROTATION -from envs.base.conftest import YAW_INPUT - - -@pytest.mark.integration -@pytest.mark.parametrize("rpy", [(0, 0, 0), (0, 0, 0.1)], indirect=True) -@pytest.mark.parametrize("vec_omega", [np.zeros(4)], indirect=True) -def test_zero_input(vec_omega, lineardroneenv): - """ - Expect it to drop like a stone - """ - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(vec_omega**2) - assert np.allclose(np.sign(obs.vel), (0, 0, -1)) - - -@pytest.mark.integration -@LOW_INPUT -def test_low_input(vec_omega, lineardroneenv): - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(vec_omega**2) - assert np.allclose(np.sign(obs.vel), (0, 0, -1)) - - -@pytest.mark.integration -@LARGE_INPUT -def test_large_input(vec_omega, lineardroneenv): - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(vec_omega**2) - assert np.allclose(np.sign(obs.vel), (0, 0, 1)) - - -@pytest.mark.integration -@ROLL_INPUT -def test_roll_input(vec_omega, lineardroneenv, exp): - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(vec_omega**2) - assert np.allclose(np.sign(obs.ang_vel), exp) - - -@pytest.mark.integration -@PITCH_INPUT -def test_pitch_input(vec_omega, lineardroneenv, exp): - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(vec_omega**2) - assert np.allclose(np.sign(obs.ang_vel), exp) - - -@pytest.mark.integration -@YAW_INPUT -def test_yaw_input(vec_omega, equilibrium_prop_rpm, lineardroneenv, exp): - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(vec_omega**2) - assert np.allclose(np.sign(obs.ang_vel), exp) - - -@pytest.mark.integration -@VELOCITY_FROM_ROTATION -def test_vel_from_rot(vec_omega, lineardroneenv, exp): - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(0.99 * vec_omega**2) - assert np.allclose(np.sign(obs.vel.round(16)), exp) - - -@pytest.mark.integration -@POSITION_FROM_VELOCITY_1 -@POSITION_FROM_VELOCITY_2 -def test_pos_from_vel(vec_omega, lineardroneenv, velocity): - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(vec_omega**2) - assert np.allclose(np.sign(obs.pos), np.sign(velocity)) - - -@pytest.mark.integration -@RPY_FROM_ANG_VEL -def test_rpy_from_ang_vel(vec_omega, lineardroneenv, angular_velocity): - lineardroneenv.reset() - obs, *_ = lineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.rpy), np.sign(angular_velocity)) - - -@pytest.mark.integration -@INPUT_TO_ROT -def test_input_to_rot(seed, lineardroneenv, action, k_Q, ang_vel_sign): - """ - Step input over a short time will give a good insight if the drone is behaving - as expected - """ - lineardroneenv.reset(seed=seed) - for _ in range(5): - obs, *_ = lineardroneenv.step(action * 100) - # Drone landed within 10cm of where we expected it - assert np.allclose(np.sign(obs.ang_vel), ang_vel_sign) diff --git a/tests/envs/base/test_nonlinear_drone.py b/tests/envs/base/test_nonlinear_drone.py deleted file mode 100644 index 9a09965..0000000 --- a/tests/envs/base/test_nonlinear_drone.py +++ /dev/null @@ -1,115 +0,0 @@ -# Copyright 2023 Jan-Hendrik Ewers -# SPDX-License-Identifier: GPL-3.0-only -import numpy as np -import pytest -from envs.base.conftest import INPUT_TO_ROT -from envs.base.conftest import LARGE_INPUT -from envs.base.conftest import LOW_INPUT -from envs.base.conftest import PITCH_INPUT -from envs.base.conftest import POSITION_FROM_VELOCITY_1 -from envs.base.conftest import POSITION_FROM_VELOCITY_2 -from envs.base.conftest import ROLL_INPUT -from envs.base.conftest import RPY_FROM_ANG_VEL -from envs.base.conftest import VELOCITY_FROM_ROTATION -from envs.base.conftest import YAW_INPUT - - -@pytest.mark.integration -@pytest.mark.parametrize( - "rpy", [(0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 1, 1)], indirect=True -) -@pytest.mark.parametrize("vec_omega", [np.zeros(4)], indirect=True) -def test_zero_input(vec_omega, nonlineardroneenv): - """ - Expect it to drop like a stone - """ - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.vel), (0, 0, -1)) - - -@pytest.mark.integration -@LOW_INPUT -def test_low_input(vec_omega, nonlineardroneenv): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.vel), (0, 0, -1)) - - -@pytest.mark.integration -@LARGE_INPUT -def test_large_input(vec_omega, nonlineardroneenv): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.vel), (0, 0, 1)) - - -@pytest.mark.integration -@pytest.mark.parametrize("vec_omega", [np.ones(4)], indirect=True) -def test_hover_input(vec_omega, nonlineardroneenv): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(obs.vel, (0, 0, 0)) - - -@pytest.mark.integration -@ROLL_INPUT -def test_roll_input(vec_omega, nonlineardroneenv, exp): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.ang_vel), exp) - - -@pytest.mark.integration -@PITCH_INPUT -def test_pitch_input(vec_omega, nonlineardroneenv, exp): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.ang_vel), exp) - - -@pytest.mark.integration -@YAW_INPUT -def test_yaw_input(vec_omega, equilibrium_prop_rpm, nonlineardroneenv, exp): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.ang_vel), exp) - - -@pytest.mark.integration -@VELOCITY_FROM_ROTATION -def test_vel_from_rot(vec_omega, nonlineardroneenv, exp): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(0.99 * vec_omega) - assert np.allclose(np.sign(obs.vel.round(16)), exp) - - -@pytest.mark.integration -@POSITION_FROM_VELOCITY_1 -@POSITION_FROM_VELOCITY_2 -def test_pos_from_vel(vec_omega, nonlineardroneenv, velocity): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.pos), np.sign(velocity)) - - -@pytest.mark.integration -@RPY_FROM_ANG_VEL -def test_rpy_from_ang_vel(vec_omega, nonlineardroneenv, angular_velocity): - nonlineardroneenv.reset() - obs, *_ = nonlineardroneenv.step(vec_omega) - assert np.allclose(np.sign(obs.rpy), angular_velocity) - - -@pytest.mark.integration -@INPUT_TO_ROT -def test_input_to_rot(seed, nonlineardroneenv, action, k_Q, ang_vel_sign): - """ - Step input over a short time will give a good insight if the drone is behaving - as expected - """ - nonlineardroneenv.reset(seed=seed) - for _ in range(5): - obs, *_ = nonlineardroneenv.step(action * 100) - # Drone landed within 10cm of where we expected it - assert np.allclose(np.sign(obs.ang_vel), ang_vel_sign) diff --git a/tests/envs/base/conftest.py b/tests/envs/conftest.py similarity index 100% rename from tests/envs/base/conftest.py rename to tests/envs/conftest.py diff --git a/tests/envs/quad/test_linear_drone.py b/tests/envs/quad/test_linear_drone.py new file mode 100644 index 0000000..67de9e6 --- /dev/null +++ b/tests/envs/quad/test_linear_drone.py @@ -0,0 +1,105 @@ +# Copyright 2023 Jan-Hendrik Ewers +# SPDX-License-Identifier: GPL-3.0-only +import numpy as np +import pytest +from envs.conftest import INPUT_TO_ROT +from envs.conftest import LARGE_INPUT +from envs.conftest import LOW_INPUT +from envs.conftest import PITCH_INPUT +from envs.conftest import POSITION_FROM_VELOCITY_1 +from envs.conftest import POSITION_FROM_VELOCITY_2 +from envs.conftest import ROLL_INPUT +from envs.conftest import RPY_FROM_ANG_VEL +from envs.conftest import VELOCITY_FROM_ROTATION +from envs.conftest import YAW_INPUT + + +@pytest.mark.integration +@pytest.mark.parametrize("rpy", [(0, 0, 0), (0, 0, 0.1)], indirect=True) +@pytest.mark.parametrize("vec_omega", [np.zeros(4)], indirect=True) +def test_zero_input(vec_omega, quadlineardroneenv): + """ + Expect it to drop like a stone + """ + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(vec_omega**2) + assert np.allclose(np.sign(obs.vel), (0, 0, -1)) + + +@pytest.mark.integration +@LOW_INPUT +def test_low_input(vec_omega, quadlineardroneenv): + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(vec_omega**2) + assert np.allclose(np.sign(obs.vel), (0, 0, -1)) + + +@pytest.mark.integration +@LARGE_INPUT +def test_large_input(vec_omega, quadlineardroneenv): + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(vec_omega**2) + assert np.allclose(np.sign(obs.vel), (0, 0, 1)) + + +@pytest.mark.integration +@ROLL_INPUT +def test_roll_input(vec_omega, quadlineardroneenv, exp): + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(vec_omega**2) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@PITCH_INPUT +def test_pitch_input(vec_omega, quadlineardroneenv, exp): + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(vec_omega**2) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@YAW_INPUT +def test_yaw_input(vec_omega, equilibrium_prop_rpm, quadlineardroneenv, exp): + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(vec_omega**2) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@VELOCITY_FROM_ROTATION +def test_vel_from_rot(vec_omega, quadlineardroneenv, exp): + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(0.99 * vec_omega**2) + assert np.allclose(np.sign(obs.vel.round(16)), exp) + + +@pytest.mark.integration +@POSITION_FROM_VELOCITY_1 +@POSITION_FROM_VELOCITY_2 +def test_pos_from_vel(vec_omega, quadlineardroneenv, velocity): + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(vec_omega**2) + assert np.allclose(np.sign(obs.pos), np.sign(velocity)) + + +@pytest.mark.integration +@RPY_FROM_ANG_VEL +def test_rpy_from_ang_vel(vec_omega, quadlineardroneenv, angular_velocity): + quadlineardroneenv.reset() + obs, *_ = quadlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.rpy), np.sign(angular_velocity)) + + +@pytest.mark.integration +@INPUT_TO_ROT +def test_input_to_rot(seed, quadlineardroneenv, action, k_Q, ang_vel_sign): + """ + Step input over a short time will give a good insight if the drone is behaving + as expected + """ + quadlineardroneenv.reset(seed=seed) + for _ in range(5): + obs, *_ = quadlineardroneenv.step(action * 100) + # Drone landed within 10cm of where we expected it + assert np.allclose(np.sign(obs.ang_vel), ang_vel_sign) diff --git a/tests/envs/quad/test_nonlinear_drone.py b/tests/envs/quad/test_nonlinear_drone.py new file mode 100644 index 0000000..5992703 --- /dev/null +++ b/tests/envs/quad/test_nonlinear_drone.py @@ -0,0 +1,115 @@ +# Copyright 2023 Jan-Hendrik Ewers +# SPDX-License-Identifier: GPL-3.0-only +import numpy as np +import pytest +from envs.conftest import INPUT_TO_ROT +from envs.conftest import LARGE_INPUT +from envs.conftest import LOW_INPUT +from envs.conftest import PITCH_INPUT +from envs.conftest import POSITION_FROM_VELOCITY_1 +from envs.conftest import POSITION_FROM_VELOCITY_2 +from envs.conftest import ROLL_INPUT +from envs.conftest import RPY_FROM_ANG_VEL +from envs.conftest import VELOCITY_FROM_ROTATION +from envs.conftest import YAW_INPUT + + +@pytest.mark.integration +@pytest.mark.parametrize( + "rpy", [(0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 1, 1)], indirect=True +) +@pytest.mark.parametrize("vec_omega", [np.zeros(4)], indirect=True) +def test_zero_input(vec_omega, quadnonlineardroneenv): + """ + Expect it to drop like a stone + """ + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.vel), (0, 0, -1)) + + +@pytest.mark.integration +@LOW_INPUT +def test_low_input(vec_omega, quadnonlineardroneenv): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.vel), (0, 0, -1)) + + +@pytest.mark.integration +@LARGE_INPUT +def test_large_input(vec_omega, quadnonlineardroneenv): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.vel), (0, 0, 1)) + + +@pytest.mark.integration +@pytest.mark.parametrize("vec_omega", [np.ones(4)], indirect=True) +def test_hover_input(vec_omega, quadnonlineardroneenv): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(obs.vel, (0, 0, 0)) + + +@pytest.mark.integration +@ROLL_INPUT +def test_roll_input(vec_omega, quadnonlineardroneenv, exp): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@PITCH_INPUT +def test_pitch_input(vec_omega, quadnonlineardroneenv, exp): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@YAW_INPUT +def test_yaw_input(vec_omega, equilibrium_prop_rpm, quadnonlineardroneenv, exp): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@VELOCITY_FROM_ROTATION +def test_vel_from_rot(vec_omega, quadnonlineardroneenv, exp): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(0.99 * vec_omega) + assert np.allclose(np.sign(obs.vel.round(16)), exp) + + +@pytest.mark.integration +@POSITION_FROM_VELOCITY_1 +@POSITION_FROM_VELOCITY_2 +def test_pos_from_vel(vec_omega, quadnonlineardroneenv, velocity): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.pos), np.sign(velocity)) + + +@pytest.mark.integration +@RPY_FROM_ANG_VEL +def test_rpy_from_ang_vel(vec_omega, quadnonlineardroneenv, angular_velocity): + quadnonlineardroneenv.reset() + obs, *_ = quadnonlineardroneenv.step(vec_omega) + assert np.allclose(np.sign(obs.rpy), angular_velocity) + + +@pytest.mark.integration +@INPUT_TO_ROT +def test_input_to_rot(seed, quadnonlineardroneenv, action, k_Q, ang_vel_sign): + """ + Step input over a short time will give a good insight if the drone is behaving + as expected + """ + quadnonlineardroneenv.reset(seed=seed) + for _ in range(5): + obs, *_ = quadnonlineardroneenv.step(action * 100) + # Drone landed within 10cm of where we expected it + assert np.allclose(np.sign(obs.ang_vel), ang_vel_sign) diff --git a/tests/envs/base/test_pybullet3_drone.py b/tests/envs/quad/test_pybullet3_drone.py similarity index 62% rename from tests/envs/base/test_pybullet3_drone.py rename to tests/envs/quad/test_pybullet3_drone.py index b8c23ae..066105d 100644 --- a/tests/envs/base/test_pybullet3_drone.py +++ b/tests/envs/quad/test_pybullet3_drone.py @@ -2,16 +2,16 @@ # SPDX-License-Identifier: GPL-3.0-only import numpy as np import pytest -from envs.base.conftest import INPUT_TO_ROT -from envs.base.conftest import LARGE_INPUT -from envs.base.conftest import LOW_INPUT -from envs.base.conftest import PITCH_INPUT -from envs.base.conftest import POSITION_FROM_VELOCITY_1_PB -from envs.base.conftest import POSITION_FROM_VELOCITY_2 -from envs.base.conftest import ROLL_INPUT -from envs.base.conftest import RPY_FROM_ANG_VEL -from envs.base.conftest import VELOCITY_FROM_ROTATION -from envs.base.conftest import YAW_INPUT +from envs.conftest import INPUT_TO_ROT +from envs.conftest import LARGE_INPUT +from envs.conftest import LOW_INPUT +from envs.conftest import PITCH_INPUT +from envs.conftest import POSITION_FROM_VELOCITY_1_PB +from envs.conftest import POSITION_FROM_VELOCITY_2 +from envs.conftest import ROLL_INPUT +from envs.conftest import RPY_FROM_ANG_VEL +from envs.conftest import VELOCITY_FROM_ROTATION +from envs.conftest import YAW_INPUT @pytest.mark.parametrize( @@ -39,10 +39,10 @@ indirect=["velocity"], ) def test_calculate_aerodynamic_forces( - pbdroneenv, drag_coeffs, rpy, state, vec_omega, exp_f + quadpbdroneenv, drag_coeffs, rpy, state, vec_omega, exp_f ): - pbdroneenv.state = state - act_f = pbdroneenv.calculate_aerodynamic_forces(vec_omega) + quadpbdroneenv.state = state + act_f = quadpbdroneenv.calculate_aerodynamic_forces(vec_omega) assert np.allclose(act_f, exp_f) @@ -65,9 +65,9 @@ def test_calculate_aerodynamic_forces( ], indirect=["vec_omega"], ) -def test_calculate_external_torques(pbdroneenv, state, vec_omega, k_Q, exp_q_z): - pbdroneenv.state = state - act_q = pbdroneenv.calculate_external_torques(vec_omega) +def test_calculate_external_torques(quadpbdroneenv, state, vec_omega, k_Q, exp_q_z): + quadpbdroneenv.state = state + act_q = quadpbdroneenv.calculate_external_torques(vec_omega) assert np.allclose(act_q, [0, 0, exp_q_z * k_Q]) @@ -89,9 +89,9 @@ def test_calculate_external_torques(pbdroneenv, state, vec_omega, k_Q, exp_q_z): ], indirect=["vec_omega"], ) -def test_calculate_propulsive_forces(pbdroneenv, state, vec_omega, k_T, exp_t): - pbdroneenv.state = state - act_t = pbdroneenv.calculate_propulsive_forces(vec_omega) +def test_calculate_propulsive_forces(quadpbdroneenv, state, vec_omega, k_T, exp_t): + quadpbdroneenv.state = state + act_t = quadpbdroneenv.calculate_propulsive_forces(vec_omega) assert np.allclose(act_t, exp_t) @@ -108,12 +108,12 @@ def test_calculate_propulsive_forces(pbdroneenv, state, vec_omega, k_T, exp_t): indirect=True, ) @pytest.mark.parametrize("vec_omega", [np.zeros(4)], indirect=True) -def test_zero_input(vec_omega, rpy, pbdroneenv): +def test_zero_input(vec_omega, rpy, quadpbdroneenv): """ Expect it to drop like a stone """ - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(vec_omega) + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(vec_omega) # Need to round, as tiny errors are introduced by PB3. We only care about bigger # picture here act = obs.vel.round(20) @@ -122,58 +122,58 @@ def test_zero_input(vec_omega, rpy, pbdroneenv): @pytest.mark.integration @LOW_INPUT -def test_low_input(vec_omega, pbdroneenv): - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(vec_omega) +def test_low_input(vec_omega, quadpbdroneenv): + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(vec_omega) assert np.allclose(np.sign(obs.vel), (0, 0, -1)) @pytest.mark.integration @LARGE_INPUT -def test_large_input(vec_omega, pbdroneenv): - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(vec_omega) +def test_large_input(vec_omega, quadpbdroneenv): + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(vec_omega) assert np.allclose(np.sign(obs.vel), (0, 0, 1)) @pytest.mark.integration @ROLL_INPUT -def test_roll_input(vec_omega, pbdroneenv, exp): - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(vec_omega) +def test_roll_input(vec_omega, quadpbdroneenv, exp): + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(vec_omega) assert np.allclose(np.sign(obs.ang_vel), exp) @pytest.mark.integration @PITCH_INPUT -def test_pitch_input(vec_omega, pbdroneenv, exp): - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(vec_omega) +def test_pitch_input(vec_omega, quadpbdroneenv, exp): + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(vec_omega) assert np.allclose(np.sign(obs.ang_vel), np.sign(exp)) @pytest.mark.integration @YAW_INPUT -def test_yaw_input(vec_omega, equilibrium_prop_rpm, pbdroneenv, exp): - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(vec_omega) +def test_yaw_input(vec_omega, equilibrium_prop_rpm, quadpbdroneenv, exp): + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(vec_omega) assert np.allclose(np.sign(obs.ang_vel), exp) @pytest.mark.integration @VELOCITY_FROM_ROTATION -def test_vel_from_rot(vec_omega, rpy, pbdroneenv, exp): - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(0.99 * vec_omega) +def test_vel_from_rot(vec_omega, rpy, quadpbdroneenv, exp): + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(0.99 * vec_omega) assert np.allclose(np.sign(obs.vel.round(4)), exp) @pytest.mark.integration @POSITION_FROM_VELOCITY_1_PB @POSITION_FROM_VELOCITY_2 -def test_pos_from_vel(pos, vec_omega, pbdroneenv, velocity): - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(vec_omega) +def test_pos_from_vel(pos, vec_omega, quadpbdroneenv, velocity): + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(vec_omega) # Need to round, as tiny errors are introduced by PB3. We only care about bigger # picture here act = obs.pos.round(5) @@ -182,9 +182,9 @@ def test_pos_from_vel(pos, vec_omega, pbdroneenv, velocity): @pytest.mark.integration @RPY_FROM_ANG_VEL -def test_rpy_from_ang_vel(vec_omega, pbdroneenv, angular_velocity): - pbdroneenv.reset() - obs, *_ = pbdroneenv.step(vec_omega) +def test_rpy_from_ang_vel(vec_omega, quadpbdroneenv, angular_velocity): + quadpbdroneenv.reset() + obs, *_ = quadpbdroneenv.step(vec_omega) # Need to round, as tiny errors are introduced by PB3. We only care about bigger # picture here act = obs.rpy.round(4) @@ -193,13 +193,13 @@ def test_rpy_from_ang_vel(vec_omega, pbdroneenv, angular_velocity): @pytest.mark.integration @INPUT_TO_ROT -def test_correct_input_to_rot(seed, pbdroneenv, action, k_Q, ang_vel_sign): +def test_correct_input_to_rot(seed, quadpbdroneenv, action, k_Q, ang_vel_sign): """ Step input over a short time will give a good insight if the drone is behaving as expected """ - pbdroneenv.reset(seed=seed) + quadpbdroneenv.reset(seed=seed) for _ in range(5): - obs, *_ = pbdroneenv.step(action * 100) + obs, *_ = quadpbdroneenv.step(action * 100) # Drone landed within 10cm of where we expected it assert np.allclose(np.sign(obs.ang_vel), ang_vel_sign) diff --git a/tests/test_gym_api.py b/tests/test_gym_api.py index 4e44dbe..515f013 100644 --- a/tests/test_gym_api.py +++ b/tests/test_gym_api.py @@ -9,9 +9,10 @@ @pytest.mark.parametrize( "env,kwargs", [ - ("PyBulletDroneEnv-v0", {}), - ("NonLinearDynamicModelDroneEnv-v0", {}), - ("LinearDynamicModelDroneEnv-v0", {}), + ("QuadPyBulletDroneEnv-v0", {}), + ("QuadNonLinearDynamicModelDroneEnv-v0", {}), + ("QuadLinearDynamicModelDroneEnv-v0", {}), + ("XWingNonlinearDynamicModelDroneEnv-v0", {}), ("LQRDroneEnv-v0", {}), ("LQRPositionDroneEnv-v0", {}), ("PolyPositionDroneEnv-v0", {}), @@ -22,18 +23,23 @@ def test_make(env, kwargs): @pytest.mark.integration -def test_PB3DroneEnv(pbdroneenv): - check_env(pbdroneenv) +def test_QuadPB3DroneEnv(quadpbdroneenv): + check_env(quadpbdroneenv) @pytest.mark.integration -def test_LinearDynamicsDroneEnv(lineardroneenv): - check_env(lineardroneenv) +def test_QuadLinearDynamicsDroneEnv(quadlineardroneenv): + check_env(quadlineardroneenv) @pytest.mark.integration -def test_NonLinearDynamicsDroneEnv(nonlineardroneenv): - check_env(nonlineardroneenv) +def test_QuadNonLinearDynamicsDroneEnv(quadnonlineardroneenv): + check_env(quadnonlineardroneenv) + + +@pytest.mark.integration +def test_XWingNonLinearDynamicsDroneEnv(xwingnonlineardroneenv): + check_env(xwingnonlineardroneenv) @pytest.mark.integration From 1a90d615e0f28cfe8c38b59abd141cf6c32bae32 Mon Sep 17 00:00:00 2001 From: iwishiwasaneagle Date: Mon, 6 Mar 2023 17:44:34 +0000 Subject: [PATCH 3/8] test: Full test x-wing NL model --- tests/envs/x_wing/test_nonlinear_drone.py | 133 ++++++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 tests/envs/x_wing/test_nonlinear_drone.py diff --git a/tests/envs/x_wing/test_nonlinear_drone.py b/tests/envs/x_wing/test_nonlinear_drone.py new file mode 100644 index 0000000..25de3a7 --- /dev/null +++ b/tests/envs/x_wing/test_nonlinear_drone.py @@ -0,0 +1,133 @@ +# Copyright 2023 Jan-Hendrik Ewers +# SPDX-License-Identifier: GPL-3.0-only +import numpy as np +import pytest +from envs.conftest import INPUT_TO_ROT +from envs.conftest import LARGE_INPUT +from envs.conftest import LOW_INPUT +from envs.conftest import PITCH_INPUT +from envs.conftest import POSITION_FROM_VELOCITY_1 +from envs.conftest import POSITION_FROM_VELOCITY_2 +from envs.conftest import ROLL_INPUT +from envs.conftest import RPY_FROM_ANG_VEL +from envs.conftest import VELOCITY_FROM_ROTATION +from envs.conftest import YAW_INPUT + + +@pytest.mark.integration +@pytest.mark.parametrize( + "rpy", [(0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 1, 1)], indirect=True +) +@pytest.mark.parametrize("vec_omega", [np.zeros(4)], indirect=True) +def test_zero_input(vec_omega, xwingnonlineardroneenv): + """ + Expect it to drop like a stone + """ + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(np.sign(obs.vel), (0, 0, -1)) + + +@pytest.mark.integration +@LOW_INPUT +def test_low_input(vec_omega, xwingnonlineardroneenv): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(np.sign(obs.vel), (0, 0, -1)) + + +@pytest.mark.integration +@LARGE_INPUT +def test_large_input(vec_omega, xwingnonlineardroneenv): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(np.sign(obs.vel), (0, 0, 1)) + + +@pytest.mark.integration +@pytest.mark.parametrize("vec_omega", [np.ones(4)], indirect=True) +def test_hover_input(vec_omega, xwingnonlineardroneenv): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(obs.vel, (0, 0, 0)) + + +@pytest.mark.integration +@ROLL_INPUT +def test_roll_input(vec_omega, xwingnonlineardroneenv, exp): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@PITCH_INPUT +def test_pitch_input(vec_omega, xwingnonlineardroneenv, exp): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@YAW_INPUT +def test_yaw_input(vec_omega, equilibrium_prop_rpm, xwingnonlineardroneenv, exp): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(np.sign(obs.ang_vel), exp) + + +@pytest.mark.integration +@VELOCITY_FROM_ROTATION +def test_vel_from_rot(vec_omega, xwingnonlineardroneenv, exp): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*0.99 * vec_omega, 0]) + assert np.allclose(np.sign(obs.vel.round(16)), exp) + + +@pytest.mark.integration +@POSITION_FROM_VELOCITY_1 +@POSITION_FROM_VELOCITY_2 +def test_pos_from_vel(vec_omega, xwingnonlineardroneenv, velocity): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(np.sign(obs.pos), np.sign(velocity)) + + +@pytest.mark.integration +@RPY_FROM_ANG_VEL +def test_rpy_from_ang_vel(vec_omega, xwingnonlineardroneenv, angular_velocity): + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, 0]) + assert np.allclose(np.sign(obs.rpy), angular_velocity) + + +@pytest.mark.integration +@INPUT_TO_ROT +def test_input_to_rot(seed, xwingnonlineardroneenv, action, k_Q, ang_vel_sign): + """ + Step input over a short time will give a good insight if the drone is behaving + as expected + """ + xwingnonlineardroneenv.reset(seed=seed) + for _ in range(5): + obs, *_ = xwingnonlineardroneenv.step([*action * 100, 0]) + # Drone landed within 10cm of where we expected it + assert np.allclose(np.sign(obs.ang_vel), ang_vel_sign) + + +@pytest.mark.integration +@pytest.mark.parametrize( + "alpha,exp", + [ + [0, (0, 0, 0)], + [1, (0, 0, 1)], + [-1, (0, 0, -1)], + ], +) +def test_alpha_input(xwingnonlineardroneenv, alpha, vec_omega, exp): + """ + Unique to this drone. Test it spins correctly with alpha + """ + xwingnonlineardroneenv.reset() + obs, *_ = xwingnonlineardroneenv.step([*vec_omega, alpha]) + assert np.allclose(np.sign(obs.ang_vel), exp) From 123b97e2b4d5dbde16c093cd11eec874949ec881 Mon Sep 17 00:00:00 2001 From: iwishiwasaneagle Date: Mon, 6 Mar 2023 17:46:00 +0000 Subject: [PATCH 4/8] fix: Name collision within pytest --- .../envs/quad/{test_linear_drone.py => test_quad_linear_drone.py} | 0 .../{test_nonlinear_drone.py => test_quad_nonlinear_drone.py} | 0 .../{test_pybullet3_drone.py => test_quad_pybullet3_drone.py} | 0 .../{test_nonlinear_drone.py => test_x_wing_nonlinear_drone.py} | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename tests/envs/quad/{test_linear_drone.py => test_quad_linear_drone.py} (100%) rename tests/envs/quad/{test_nonlinear_drone.py => test_quad_nonlinear_drone.py} (100%) rename tests/envs/quad/{test_pybullet3_drone.py => test_quad_pybullet3_drone.py} (100%) rename tests/envs/x_wing/{test_nonlinear_drone.py => test_x_wing_nonlinear_drone.py} (100%) diff --git a/tests/envs/quad/test_linear_drone.py b/tests/envs/quad/test_quad_linear_drone.py similarity index 100% rename from tests/envs/quad/test_linear_drone.py rename to tests/envs/quad/test_quad_linear_drone.py diff --git a/tests/envs/quad/test_nonlinear_drone.py b/tests/envs/quad/test_quad_nonlinear_drone.py similarity index 100% rename from tests/envs/quad/test_nonlinear_drone.py rename to tests/envs/quad/test_quad_nonlinear_drone.py diff --git a/tests/envs/quad/test_pybullet3_drone.py b/tests/envs/quad/test_quad_pybullet3_drone.py similarity index 100% rename from tests/envs/quad/test_pybullet3_drone.py rename to tests/envs/quad/test_quad_pybullet3_drone.py diff --git a/tests/envs/x_wing/test_nonlinear_drone.py b/tests/envs/x_wing/test_x_wing_nonlinear_drone.py similarity index 100% rename from tests/envs/x_wing/test_nonlinear_drone.py rename to tests/envs/x_wing/test_x_wing_nonlinear_drone.py From 16da2585a90b52b2d19df46517831e74c46fddd3 Mon Sep 17 00:00:00 2001 From: iwishiwasaneagle Date: Tue, 7 Mar 2023 09:29:57 +0000 Subject: [PATCH 5/8] fix: Clean up x-wing dstate code --- src/jdrones/envs/x_wing/nonlineardroneenv.py | 28 +++++++++++--------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/jdrones/envs/x_wing/nonlineardroneenv.py b/src/jdrones/envs/x_wing/nonlineardroneenv.py index efcc95b..1a819cd 100644 --- a/src/jdrones/envs/x_wing/nonlineardroneenv.py +++ b/src/jdrones/envs/x_wing/nonlineardroneenv.py @@ -37,28 +37,31 @@ def calc_dstate(action: VEC5, state: State, model: URDFModel): m = model.mass g = model.g length = model.l - alpha = action[4] - - T1, T2, T3, T4 = model.k_T * np.square(action[:4]) - u_star = model.rpm2rpyT(np.square(action[:4])) - unit_z = np.array([0, 0, 1]).reshape((-1, 1)) + alpha = action[4] + p_squared = np.square(action[:4]) + T1, T2, T3, T4 = model.k_T * p_squared + u_star = model.rpm2rpyT(p_squared) + tau_phi, tau_theta, tau_psi, thrust = u_star + unit_z = np.array([0, 0, 1]) R_W_B = np.array(euler_to_rotmat(state.rpy)) + salpha, calpha = np.sin(alpha), np.cos(alpha) + dstate = np.concatenate( [ state.vel, (0, 0, 0, 0), state.ang_vel, ( - -m * g * unit_z.T + -m * g * unit_z + ( R_W_B @ [ - T4 * np.sin(alpha) + T2 * np.sin(-alpha), - T1 * np.sin(alpha) + T4 * np.sin(-alpha), - u_star[3] * np.cos(alpha), + (T4 - T2) * salpha, + (T1 - T4) * salpha, + thrust * calpha, ] ).T ).flatten() @@ -68,10 +71,9 @@ def calc_dstate(action: VEC5, state: State, model: URDFModel): ( R_W_B @ [ - u_star[0] * np.cos(alpha), - u_star[1] * np.cos(alpha), - length * u_star[3] * np.sin(alpha) - + u_star[2] * np.cos(alpha), + tau_phi * calpha, + tau_theta * calpha, + length * thrust * salpha + tau_psi * calpha, ] ), ), From 0479b855d50dff34dc63dbaccfdda850b42c262b Mon Sep 17 00:00:00 2001 From: iwishiwasaneagle Date: Tue, 7 Mar 2023 09:34:05 +0000 Subject: [PATCH 6/8] fix: Typo --- src/jdrones/envs/x_wing/nonlineardroneenv.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/jdrones/envs/x_wing/nonlineardroneenv.py b/src/jdrones/envs/x_wing/nonlineardroneenv.py index 1a819cd..d10367e 100644 --- a/src/jdrones/envs/x_wing/nonlineardroneenv.py +++ b/src/jdrones/envs/x_wing/nonlineardroneenv.py @@ -60,7 +60,7 @@ def calc_dstate(action: VEC5, state: State, model: URDFModel): R_W_B @ [ (T4 - T2) * salpha, - (T1 - T4) * salpha, + (T1 - T3) * salpha, thrust * calpha, ] ).T From 1a722c351ddd7563bc5f251342baeca54e166040 Mon Sep 17 00:00:00 2001 From: iwishiwasaneagle Date: Tue, 7 Mar 2023 10:03:37 +0000 Subject: [PATCH 7/8] docs: Update maths, and centralize mixing model maths around a single function's docstring --- docs/envs.rst | 11 +++++- src/jdrones/envs/dronemodels.py | 33 +++++++++++++++++ src/jdrones/envs/quad/lineardronenev.py | 23 +++--------- src/jdrones/envs/quad/nonlineardronenev.py | 10 +++--- src/jdrones/envs/x_wing/nonlineardroneenv.py | 37 ++++++++++++++++++++ 5 files changed, 90 insertions(+), 24 deletions(-) diff --git a/docs/envs.rst b/docs/envs.rst index 2c464fe..ff69e94 100644 --- a/docs/envs.rst +++ b/docs/envs.rst @@ -35,7 +35,7 @@ QuadLinearDynamicModelDroneEnv XWingNonLinearDynamicModelDroneEnv ---------------------------------- -.. autoclass:: jdrones.envs.XWingNonLinearDynamicModelDroneEnv +.. autoclass:: jdrones.envs.XWingNonlinearDynamicModelDroneEnv :members: :undoc-members: :show-inheritance: @@ -80,3 +80,12 @@ PolyPositionDroneEnv :members: :undoc-members: :show-inheritance: + +Models +====== + + +.. automodule:: jdrones.envs.dronemodels + :members: + :undoc-members: + :show-inheritance: diff --git a/src/jdrones/envs/dronemodels.py b/src/jdrones/envs/dronemodels.py index 40c3172..21bacb9 100644 --- a/src/jdrones/envs/dronemodels.py +++ b/src/jdrones/envs/dronemodels.py @@ -7,6 +7,39 @@ def droneplus_mixing_matrix(*, length, k_Q, k_T): + """ + .. 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 + ------- + VEC4X4 + Mixing matrix + """ + return np.array( [ [0, -k_T * length, 0, k_T * length], diff --git a/src/jdrones/envs/quad/lineardronenev.py b/src/jdrones/envs/quad/lineardronenev.py index e61ad48..a69b045 100644 --- a/src/jdrones/envs/quad/lineardronenev.py +++ b/src/jdrones/envs/quad/lineardronenev.py @@ -119,7 +119,7 @@ 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\\\\ @@ -127,28 +127,15 @@ def step(self, action: PropellerAction) -> Tuple[State, float, bool, bool, dict] 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 ---------- diff --git a/src/jdrones/envs/quad/nonlineardronenev.py b/src/jdrones/envs/quad/nonlineardronenev.py index 987cdea..5b5c18f 100644 --- a/src/jdrones/envs/quad/nonlineardronenev.py +++ b/src/jdrones/envs/quad/nonlineardronenev.py @@ -60,13 +60,13 @@ def step(self, action: PropellerAction) -> Tuple[State, float, bool, bool, dict] - R^B_E (\\vec C_d R^E_B \\vec x ') \\\\ \\vec I \\vec \\phi'' &= - \\begin{bmatrix} - l k_T (P_4^2-P_2^2) \\\\ - l k_T (P_1^2-P_3^2) \\\\ - k_Q (-P_1^2+P_2^2-P_3^2+P_4^2) - \\end{bmatrix} + \\vec \\Gamma \\end{align} + .. seealso:: + :math:`\\vec M = [\\vec \\Gamma, T]^T` is defined in + :func:`jdrones.envs.dronemodels.droneplus_mixing_matrix` + Parameters ---------- action : diff --git a/src/jdrones/envs/x_wing/nonlineardroneenv.py b/src/jdrones/envs/x_wing/nonlineardroneenv.py index d10367e..702cb51 100644 --- a/src/jdrones/envs/x_wing/nonlineardroneenv.py +++ b/src/jdrones/envs/x_wing/nonlineardroneenv.py @@ -83,6 +83,43 @@ def calc_dstate(action: VEC5, state: State, model: URDFModel): return dstate def step(self, action: PropellerAction) -> Tuple[State, float, bool, bool, dict]: + """ + .. math:: + \\begin{align} + m\\vec x '' &= + \\begin{bmatrix} + 0\\\\0\\\\mg + \\end{bmatrix} + - + R^B_E + \\begin{bmatrix} + (T_4-T_2)\\sin(\\alpha)\\\\ + (T_1-T_3)\\sin(\\alpha)\\\\ + \\Sigma^4_{i=1} T_i \\cos(\\alpha) + \\end{bmatrix} + - + R^B_E (\\vec C_d R^E_B \\vec x ') \\\\ + \\vec I \\vec \\phi'' &= + \\begin{bmatrix} + \\Gamma_\\phi \\cos(\\alpha) \\\\ + \\Gamma_\\theta \\cos(\\alpha)\\\\ + \\Gamma_\\psi + \\Sigma^4_{i=0} T_i \\sin(\\alpha) + \\end{bmatrix} + \\vec T_i &= k_T P_i^2 \\\\ + \\end{align} + + .. seealso:: + :math:`\\vec M = [\\vec \\Gamma, T]^T` is defined in + :func:`jdrones.envs.dronemodels.droneplus_mixing_matrix` + + Parameters + ---------- + action : + + Returns + ------- + + """ # Get state dstate = self.calc_dstate(action, self.state, self.model) From b4d358c812ed340bfca62502b882761656680403 Mon Sep 17 00:00:00 2001 From: iwishiwasaneagle Date: Tue, 7 Mar 2023 10:28:12 +0000 Subject: [PATCH 8/8] fix: Typo 4X3 -> 4X4 --- src/jdrones/envs/dronemodels.py | 5 +++-- src/jdrones/types.py | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/jdrones/envs/dronemodels.py b/src/jdrones/envs/dronemodels.py index 21bacb9..e242ac4 100644 --- a/src/jdrones/envs/dronemodels.py +++ b/src/jdrones/envs/dronemodels.py @@ -4,9 +4,10 @@ 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 @@ -36,7 +37,7 @@ def droneplus_mixing_matrix(*, length, k_Q, k_T): Returns ------- - VEC4X4 + jdrones.types.MAT4X4 Mixing matrix """ diff --git a/src/jdrones/types.py b/src/jdrones/types.py index f2c37e2..bf83a40 100644 --- a/src/jdrones/types.py +++ b/src/jdrones/types.py @@ -6,10 +6,10 @@ VEC4 = tuple[float, float, float, float] """:math:`(a,b,c,d)` vector""" MAT3X3 = tuple[VEC3, VEC3, VEC3] +""":math:`3 \\times 3` matrix""" VEC5 = tuple[float, float, float, float, float] """:math:`(a,b,c,d,e)` vector""" -""":math:`3 \\times 3` matrix""" -MAT4X3 = tuple[VEC4, VEC4, VEC4, VEC4] +MAT4X4 = tuple[VEC4, VEC4, VEC4, VEC4] """:math:`4 \\times 4` matrix""" Action = list[float] LinearXAction = tuple[