Commit 5632c5c4 authored by Ibrahim's avatar Ibrahim
Browse files

Position PID tracks velocity;

control.py: PosController uses velocity vector to make roll/pitch action outputs,
env.py: Bugfix where state attribute was not updated on calling step,
simulation.py, vehicle.py: use_thrust_constant made part of PropellerParams instead of argument to Propeller.
parent fb0fef64
......@@ -64,7 +64,8 @@ class PosController(PIDController):
vehicle: Multirotor
max_tilt: float = np.pi / 15
max_velocity: float = 7.0
max_velocity: float = 1.0
def __post_init__(self):
......@@ -92,7 +93,7 @@ class PosController(PIDController):
# actual/measured velocity
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 = super().step(reference=ref_vel_xy, measurement=mea_delta_xy)
# ctrl[0] -> x dir -> pitch -> forward
# ctrl[1] -> y dir -> roll -> lateral
ctrl[0:2] = np.clip(ctrl[0:2], a_min=-self.max_tilt, a_max=self.max_tilt)
......@@ -125,6 +126,7 @@ class AttController(PIDController):
return self.vehicle.params.inertia_matrix.dot(ctrl)
@dataclass
class AltController(PIDController):
......@@ -146,6 +148,7 @@ class AltController(PIDController):
return ctrl # thrust force
class Controller:
def __init__(self, ctrl_p: PosController, ctrl_a: AttController, ctrl_z: AltController):
......
......@@ -58,8 +58,8 @@ class DynamicsMultirotorEnv(BaseMultirotorEnv):
speeds = self.vehicle.allocate_control(action[0], action[3:6])
forces, torques = self.vehicle.get_forces_torques(speeds, self.vehicle.state)
action = np.concatenate(forces, torques)
state = self.vehicle.step_dynamics(u=action)
return state, None, None, None
self.state = self.vehicle.step_dynamics(u=action)
return self.state, None, None, None
......@@ -78,5 +78,5 @@ class SpeedsMultirotorEnv(gym.Env):
def step(self, action: np.ndarray):
state = self.vehicle.step_speeds(u=action)
return state, None, None, None
\ No newline at end of file
self.state = self.vehicle.step_speeds(u=action)
return self.state, None, None, None
\ No newline at end of file
......@@ -4,6 +4,8 @@ import numpy as np
from numba import njit
from scipy.optimize import fsolve
from .coords import body_to_inertial, direction_cosine_matrix
@njit
......@@ -106,6 +108,8 @@ def apply_forces_torques(
# velocity = dPosition (inertial) / dt (convert body velocity to inertial)
# Essentially = Rotation matrix (body to inertial) x body velocity
# dcm = direction_cosine_matrix(roll=phi, pitch=theta, yaw=psi)
# xdot[0:3] = body_to_inertial(x[3:6], dcm)
xdot[0] = cthe*cpsi*ub + (-cphi * spsi + sphi*sthe*cpsi) * vb + \
(sphi*spsi+cphi*sthe*cpsi) * wb # = xIdot
xdot[1] = cthe*spsi * ub + (cphi*cpsi+sphi*sthe*spsi) * vb + \
......
......@@ -19,16 +19,14 @@ class Propeller:
"""
def __init__(
self, params: PropellerParams, simulation: SimulationParams,
use_thrust_constant: bool=False
self, params: PropellerParams, simulation: SimulationParams
) -> None:
self.params: PropellerParams = deepcopy(params)
self.simulation: SimulationParams = simulation
self.speed: float = 0.
"Radians per second"
self.use_thrust_constant = use_thrust_constant
if use_thrust_constant:
if self.params.use_thrust_constant:
self.thrust: Callable = self._thrust_constant
else:
self.thrust: Callable = self._thrust_physics
......
......@@ -55,7 +55,8 @@ class PropellerParams:
"Mean chord length (m) of the propeller blade"
eta: float = 1.
"Propeller efficiency"
use_thrust_constant: bool = True
"Use k_thrust instead of propeller geometry for thrust calculations."
k_thrust: float = None
"Propeller's aerodynamic thrust coefficient, where thrust = k_thrust * angular velocity^2"
k_drag: float = None
......@@ -76,6 +77,8 @@ class PropellerParams:
"Pitch angle at root of blade"
self.theta1 = -4 / 3 * np.arctan2(self.pitch, 2 * np.pi * 3/4 * self.diameter/2)
"Change in pitch angle towards tip of blade"
if self.use_thrust_constant and self.k_thrust is None:
raise ValueError('"k_thrust" is None, even though "use_thrust_constant=True"')
if self.motor is not None:
self.motor.k_drag = self.k_drag
# Pitch angel is reduced to allow for even lift as blade velocity increases
......
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