Commit 19ab1d5f authored by Ibrahim's avatar Ibrahim
Browse files

simulation.py: state and derivatives' precision limited to mitigate numerical errors

parent f1bf4fb3
......@@ -151,6 +151,7 @@ class PosController(PIDController):
vehicle: Multirotor
max_tilt: float = np.pi / 15
max_velocity: float = 7.0
def __post_init__(self):
......@@ -177,8 +178,12 @@ class PosController(PIDController):
# Using rotation matrix. For a positive yaw, the target x,y will appear
# desired/reference change in x/y i.e. velocity
ref_delta_xy = rot @ np.asarray([delta_x, delta_y])
# TODO: Explicitly track velocity, instead of deltas
# ref_vel_xy = ref_delta_xy / self.vehicle.simulation.dt
# abs_max_vel = np.abs((ref_vel_xy / (np.linalg.norm(ref_vel_xy) + 1e-6)) * self.max_velocity)
# ref_vel_xy = np.clip(ref_vel_xy, a_min=-abs_max_vel, a_max=abs_max_vel)
# actual/measured velocity
mea_delta_xy = self.vehicle.velocity[:2]
mea_delta_xy = mea_vel_xy = self.vehicle.velocity[:2]
# desired pitch, roll
ctrl = super().step(reference=ref_delta_xy, measurement=mea_delta_xy)
# ctrl[0] -> x dir -> pitch -> forward
......
from typing import List, Tuple
from copy import deepcopy
import numpy as np
from numpy import (cos, sin)
......@@ -21,7 +22,7 @@ class Propeller:
self, params: PropellerParams, simulation: SimulationParams,
use_thrust_constant: bool=False
) -> None:
self.params: PropellerParams = params
self.params: PropellerParams = deepcopy(params)
self.simulation: SimulationParams = simulation
self.speed: float = 0.
"Radians per second"
......@@ -107,7 +108,7 @@ class Motor:
"""
def __init__(self, params: MotorParams, simulation: SimulationParams) -> None:
self.params = params
self.params = deepcopy(params)
self.simulation = simulation
self.speed: float = 0.
"Radians per second"
......@@ -125,7 +126,7 @@ class Motor:
class Multirotor:
def __init__(self, params: VehicleParams, simulation: SimulationParams) -> None:
self.params: VehicleParams = params
self.params: VehicleParams = deepcopy(params)
self.simulation: SimulationParams = simulation
self.state: np.ndarray = None
self.propellers: List[Propeller] = None
......@@ -227,7 +228,20 @@ class Multirotor:
return forces, torques
def dxdt(self, x: np.ndarray, t: float, u: np.ndarray):
def dxdt_dynamics(self, x: np.ndarray, t: float, u: np.ndarray):
# This method must not have any side-effects. It should not change the
# state of the vehicle. This method is called multiple times from the
# same state by the odeint() function, and the results should be consistent.
# Do not need to get forces/torques on body, since the action array
# already is a 6d vector of forces/torques.
# forces, torques = self.get_forces_torques(u, x)
xdot = apply_forces_torques(
u[:3], u[3:], x, self.simulation.g,
self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse)
return np.around(xdot, 4)
def dxdt_speeds(self, x: np.ndarray, t: float, u: np.ndarray):
# This method must not have any side-effects. It should not change the
# state of the vehicle. This method is called multiple times from the
# same state by the odeint() function, and the results should be consistent.
......@@ -235,16 +249,30 @@ class Multirotor:
xdot = apply_forces_torques(
forces, torques, x, self.simulation.g,
self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse)
return xdot
return np.around(xdot, 4)
def step_dynamics(self, u: np.ndarray):
self.t += self.simulation.dt
# TODO: Explore RK45() or solve_ivp() functions from scipy.integrate?
self.state = odeint(
self.dxdt_dynamics, self.state, (0, self.simulation.dt), args=(u,),
rtol=1e-4, atol=1e-4
)[-1]
self.state = np.around(self.state, 4)
for u_, prop in zip(u, self.propellers):
prop.step(u_)
return self.state
def step(self, u: np.ndarray):
def step_speeds(self, u: np.ndarray):
self.t += self.simulation.dt
# TODO: Explore RK45() or solve_ivp() functions from scipy.integrate?
self.state = odeint(
self.dxdt, self.state, (0, self.simulation.dt), args=(u,),
rtol=1e-5, atol=1e-5
self.dxdt_speeds, self.state, (0, self.simulation.dt), args=(u,),
rtol=1e-4, atol=1e-4
)[-1]
self.state = np.around(self.state, 4)
for u_, prop in zip(u, self.propellers):
prop.step(u_)
return self.state
......
......@@ -100,7 +100,7 @@ class VehicleParams:
@dataclass
class SimulationParams:
dt: float = 1e-2
dt: float = 1e-3
"""Timestep of simulation"""
g: float = 9.81
"""Gravitational acceleration"""
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment