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): ...@@ -151,6 +151,7 @@ class PosController(PIDController):
vehicle: Multirotor vehicle: Multirotor
max_tilt: float = np.pi / 15 max_tilt: float = np.pi / 15
max_velocity: float = 7.0
def __post_init__(self): def __post_init__(self):
...@@ -177,8 +178,12 @@ class PosController(PIDController): ...@@ -177,8 +178,12 @@ class PosController(PIDController):
# Using rotation matrix. For a positive yaw, the target x,y will appear # Using rotation matrix. For a positive yaw, the target x,y will appear
# desired/reference change in x/y i.e. velocity # desired/reference change in x/y i.e. velocity
ref_delta_xy = rot @ np.asarray([delta_x, delta_y]) 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 # actual/measured velocity
mea_delta_xy = self.vehicle.velocity[:2] mea_delta_xy = mea_vel_xy = self.vehicle.velocity[:2]
# desired pitch, roll # desired pitch, roll
ctrl = super().step(reference=ref_delta_xy, measurement=mea_delta_xy) ctrl = super().step(reference=ref_delta_xy, measurement=mea_delta_xy)
# ctrl[0] -> x dir -> pitch -> forward # ctrl[0] -> x dir -> pitch -> forward
......
from typing import List, Tuple from typing import List, Tuple
from copy import deepcopy
import numpy as np import numpy as np
from numpy import (cos, sin) from numpy import (cos, sin)
...@@ -21,7 +22,7 @@ class Propeller: ...@@ -21,7 +22,7 @@ class Propeller:
self, params: PropellerParams, simulation: SimulationParams, self, params: PropellerParams, simulation: SimulationParams,
use_thrust_constant: bool=False use_thrust_constant: bool=False
) -> None: ) -> None:
self.params: PropellerParams = params self.params: PropellerParams = deepcopy(params)
self.simulation: SimulationParams = simulation self.simulation: SimulationParams = simulation
self.speed: float = 0. self.speed: float = 0.
"Radians per second" "Radians per second"
...@@ -107,7 +108,7 @@ class Motor: ...@@ -107,7 +108,7 @@ class Motor:
""" """
def __init__(self, params: MotorParams, simulation: SimulationParams) -> None: def __init__(self, params: MotorParams, simulation: SimulationParams) -> None:
self.params = params self.params = deepcopy(params)
self.simulation = simulation self.simulation = simulation
self.speed: float = 0. self.speed: float = 0.
"Radians per second" "Radians per second"
...@@ -125,7 +126,7 @@ class Motor: ...@@ -125,7 +126,7 @@ class Motor:
class Multirotor: class Multirotor:
def __init__(self, params: VehicleParams, simulation: SimulationParams) -> None: def __init__(self, params: VehicleParams, simulation: SimulationParams) -> None:
self.params: VehicleParams = params self.params: VehicleParams = deepcopy(params)
self.simulation: SimulationParams = simulation self.simulation: SimulationParams = simulation
self.state: np.ndarray = None self.state: np.ndarray = None
self.propellers: List[Propeller] = None self.propellers: List[Propeller] = None
...@@ -227,7 +228,20 @@ class Multirotor: ...@@ -227,7 +228,20 @@ class Multirotor:
return forces, torques 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 # 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 # state of the vehicle. This method is called multiple times from the
# same state by the odeint() function, and the results should be consistent. # same state by the odeint() function, and the results should be consistent.
...@@ -235,16 +249,30 @@ class Multirotor: ...@@ -235,16 +249,30 @@ class Multirotor:
xdot = apply_forces_torques( xdot = apply_forces_torques(
forces, torques, x, self.simulation.g, forces, torques, x, self.simulation.g,
self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse) 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 self.t += self.simulation.dt
# TODO: Explore RK45() or solve_ivp() functions from scipy.integrate? # TODO: Explore RK45() or solve_ivp() functions from scipy.integrate?
self.state = odeint( self.state = odeint(
self.dxdt, self.state, (0, self.simulation.dt), args=(u,), self.dxdt_speeds, self.state, (0, self.simulation.dt), args=(u,),
rtol=1e-5, atol=1e-5 rtol=1e-4, atol=1e-4
)[-1] )[-1]
self.state = np.around(self.state, 4)
for u_, prop in zip(u, self.propellers): for u_, prop in zip(u, self.propellers):
prop.step(u_) prop.step(u_)
return self.state return self.state
......
...@@ -100,7 +100,7 @@ class VehicleParams: ...@@ -100,7 +100,7 @@ class VehicleParams:
@dataclass @dataclass
class SimulationParams: class SimulationParams:
dt: float = 1e-2 dt: float = 1e-3
"""Timestep of simulation""" """Timestep of simulation"""
g: float = 9.81 g: float = 9.81
"""Gravitational acceleration""" """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