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

control.py

+5 −2
Original line number Diff line number Diff line
@@ -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):

env.py

+4 −4
Original line number Diff line number Diff line
@@ -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

physics.py

+4 −0
Original line number Diff line number Diff line
@@ -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 + \

simulation.py

+2 −4
Original line number Diff line number Diff line
@@ -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

vehicle.py

+4 −1
Original line number Diff line number Diff line
@@ -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