Skip to content

Commit

Permalink
Merge pull request #140 from iwishiwasaneagle/libjdrones
Browse files Browse the repository at this point in the history 
feat: Adapt jdrones to be mainly run via libjdrones C++
  • Loading branch information
@iwishiwasaneagle
iwishiwasaneagle authored Aug 30, 2024
2 parents cd765bb + 9aaac5f commit aedbf10
Show file tree
Hide file tree
Showing 19 changed files with 306 additions and 701 deletions.
1 change: 1 addition & 0 deletions pyproject.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ classifiers = [
requires-python = ">=3.10"
dependencies = [
'importlib-metadata; python_version<"3.8"',
"libjdrones>=0.1.1",
"loguru",
"numpy",
"scipy",
Expand Down
1 change: 1 addition & 0 deletions requirements.txt
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
gymnasium==0.29.1
libjdrones==0.1.1
loguru==0.7.0
matplotlib==3.8.3
nptyping==2.5.0
Expand Down
1 change: 0 additions & 1 deletion src/jdrones/controllers.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,7 +201,6 @@ def __init__(self, A, B, Q, R):

self.K = self.solve(A, B, Q, R)

@staticmethod
def solve(A, B, Q, R):
"""Solve the continuous time lqr controller.
Expand Down
45 changes: 2 additions & 43 deletions src/jdrones/envs/base/basecontrolledenv.py
View file
Original file line number Diff line number Diff line change
@@ -1,48 +1,7 @@
# Copyright 2023 Jan-Hendrik Ewers
# SPDX-License-Identifier: GPL-3.0-only
import abc
from typing import Dict
from typing import Optional
from typing import Tuple

import gymnasium
from jdrones.controllers import Controller
from jdrones.controllers import PID
from jdrones.data_models import State
from jdrones.envs.base.basedronenev import BaseDroneEnv


class BaseControlledEnv(gymnasium.Env, abc.ABC):
env: BaseDroneEnv

controllers: Dict[str, PID]

def __init__(self, env: BaseDroneEnv, dt: float = 1 / 240):
self.env = env
self.dt = dt
self.controllers = self._init_controllers()

self.observation_space = self.env.observation_space

@property
def state(self):
return self.env.state

@property
def initial_state(self):
return self.env.initial_state

def reset(
self,
*,
seed: Optional[int] = None,
options: Optional[dict] = None,
) -> Tuple[State, dict]:
super().reset(seed=seed, options=options)
for ctrl in self.controllers.values():
ctrl.reset()
return self.env.reset(options=options)

@abc.abstractmethod
def _init_controllers(self) -> dict[str, Controller]:
pass
class BaseControlledEnv(gymnasium.Env):
pass
24 changes: 4 additions & 20 deletions src/jdrones/envs/base/basedronenev.py
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
# Copyright 2023 Jan-Hendrik Ewers
# SPDX-License-Identifier: GPL-3.0-only
from copy import copy
import abc
from typing import Any
from typing import Optional
from typing import Tuple
Expand All @@ -9,36 +9,27 @@
import numpy as np
from gymnasium import spaces
from jdrones.data_models import State
from jdrones.data_models import URDFModel
from jdrones.envs.dronemodels import DronePlus
from jdrones.transforms import euler_to_quat
from jdrones.types import DType


class BaseDroneEnv(gymnasium.Env):
class BaseDroneEnv(gymnasium.Env, abc.ABC):
state: State
"""Current drone state"""

initial_state: State
"""Initial drone state. Used for resettign the simulation"""

model: URDFModel
"""Model parameters"""

info: dict[str, Any]
"""Information dictionary to return"""

def __init__(
self,
model: URDFModel = DronePlus,
initial_state: State = None,
dt: float = 1 / 240,
):
if initial_state is None:
initial_state = State()
self.initial_state = initial_state
self.state = copy(self.initial_state)
self.model = model
self.dt = dt
self.info = {}

Expand Down Expand Up @@ -95,19 +86,12 @@ def __init__(
low=obs_bounds[:, 0], high=obs_bounds[:, 1], dtype=DType
)

@abc.abstractmethod
def reset(
self,
*,
seed: Optional[int] = None,
options: Optional[dict] = None,
) -> Tuple[State, dict]:
super().reset(seed=seed, options=options)
self.info = {}

if options is not None:
reset_state = options.get("reset_state", self.initial_state)
else:
reset_state = self.initial_state
self.state = copy(reset_state)
self.state.quat = euler_to_quat(self.state.rpy)
return self.state, self.info
return self.state, {}
93 changes: 25 additions & 68 deletions src/jdrones/envs/base/lineardronenev.py
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,15 @@
# Copyright 2023 Jan-Hendrik Ewers
# SPDX-License-Identifier: GPL-3.0-only
from typing import Optional
from typing import Tuple

import numba
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.envs.dronemodels import DronePlus
from jdrones.transforms import euler_to_quat
from jdrones.types import DType
from jdrones.types import PropellerAction
from libjdrones import LinearDynamicModelDroneEnv as _LinearDynamicModelDroneEnv


class LinearDynamicModelDroneEnv(BaseDroneEnv):
Expand All @@ -22,15 +20,16 @@ class LinearDynamicModelDroneEnv(BaseDroneEnv):
<OrderEnforcing<PassiveEnvChecker<LinearDynamicModelDroneEnv<LinearDynamicModelDroneEnv-v0>>>>
"""

_env: _LinearDynamicModelDroneEnv

def __init__(
self,
model: URDFModel = DronePlus,
initial_state: State = None,
dt: float = 1 / 240,
):
super().__init__(model, initial_state, dt)
self._env = _LinearDynamicModelDroneEnv(dt)

self.A, self.B, self.C = self.get_matrices(model)
super().__init__(initial_state, dt)

act_bounds = np.array(
[[-np.inf, -np.inf, -np.inf, -np.inf], [np.inf, np.inf, np.inf, np.inf]],
Expand All @@ -40,59 +39,25 @@ def __init__(
low=act_bounds[0], high=act_bounds[1], dtype=DType
)

@staticmethod
def get_matrices(model: URDFModel):
m = model.mass
g = model.g
Ix, Iy, Iz = model.I

A = np.array(
[
(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0), # x
(0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0), # y
(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0), # z
(0, 0, 0, 0, 0, 0, 0, g, 0, 0, 0, 0), # dx
(0, 0, 0, 0, 0, 0, -g, 0, 0, 0, 0, 0), # dy
(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0), # dz
(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0), # phi
(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0), # theta
(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1), # psi
(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0), # dphi
(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0), # dtheta
(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0), # dpsi
]
)

B = np.array(
[
(0, 0, 0, 0), # x
(0, 0, 0, 0), # y
(0, 0, 0, 0), # z
(0, 0, 0, 0), # dx
(0, 0, 0, 0), # dy
(0, 0, 0, 1 / m), # dz
(0, 0, 0, 0), # phi
(0, 0, 0, 0), # theta
(0, 0, 0, 0), # psi
(1 / Ix, 0, 0, 0), # dphi
(0, 1 / Iy, 0, 0), # dtheta
(0, 0, 1 / Iz, 0), # dpsi
]
)

C = np.vstack([0, 0, 0, 0, 0, -g, 0, 0, 0, 0, 0, 0])
@property
def state(self) -> State:
return State(self._env.state)

return A, B, C

@staticmethod
@numba.njit
def calc_dx(A, B, C, x, u):
return (A @ x + B @ u + C.T).flatten()

def calc_dstate(self, action) -> State:
x = self.state.to_x()
dstate = self.calc_dx(self.A, self.B, self.C, x, action)
return State.from_x(dstate)
def reset(
self,
*,
seed: Optional[int] = None,
options: Optional[dict] = None,
) -> Tuple[State, dict]:
super().reset(seed=seed, options=options)
self.info = {}

if options is not None:
reset_state = options.get("reset_state", self.initial_state)
else:
reset_state = self.initial_state
self._env.reset(reset_state)
return self.state, self.info

def step(self, action: PropellerAction) -> Tuple[State, float, bool, bool, dict]:
"""
Expand Down Expand Up @@ -175,13 +140,5 @@ def step(self, action: PropellerAction) -> Tuple[State, float, bool, bool, dict]
-------
"""
dstate = self.calc_dstate(self.model.rpm2rpyT(action))
# Update step
self.state += self.dt * dstate

# Update derived state items
self.state.prop_omega = action
self.state.quat = euler_to_quat(self.state.rpy)

# Return
self._env.step(action)
return self.state, 0, False, False, self.info
Loading

0 comments on commit aedbf10

Please sign in to comment.