control.attitude_controller

lsy_drone_racing.control.attitude_controller

This module implements an AttitudeController for quadrotor control.

It utilizes the collective thrust interface for drone control to compute control commands based on current state observations and desired waypoints. The attitude control is handled by computing a PID control law for position tracking, incorporating gravity compensation in thrust calculations.

The waypoints are generated using cubic spline interpolation from a set of predefined waypoints. Note that the trajectory uses pre-defined waypoints instead of dynamically generating a good path.

Classes

AttitudeController(obs, info, config)

Bases: Controller

Example of a controller using the collective thrust and attitude interface.

Initialize the attitude controller.

Parameters:

Name	Type	Description	Default
obs	dict[str, NDArray[floating]]	The initial observation of the environment's state. See the environment's observation space for details.	required
info	dict	Additional environment information from the reset.	required
config	dict	The configuration of the environment.	required

Source code in lsy_drone_racing/control/attitude_controller.py

def __init__(self, obs: dict[str, NDArray[np.floating]], info: dict, config: dict):
    """Initialize the attitude controller.

    Args:
        obs: The initial observation of the environment's state. See the environment's
            observation space for details.
        info: Additional environment information from the reset.
        config: The configuration of the environment.
    """
    super().__init__(obs, info, config)
    self._freq = config.env.freq

    # For more info on the models, check out https://github.com/learnsyslab/drone-models
    drone_params = load_params(config.sim.physics, config.sim.drone_model)
    self.drone_mass = drone_params["mass"]

    self.kp = np.array([0.4, 0.4, 1.25])
    self.ki = np.array([0.05, 0.05, 0.05])
    self.kd = np.array([0.2, 0.2, 0.4])
    self.ki_range = np.array([2.0, 2.0, 0.4])
    self.i_error = np.zeros(3)
    self.g = 9.81

    # Same waypoints as in the state controller. Determined by trial and error.
    start_pos = obs["pos"]
    waypoints = np.array(
        [
            start_pos,
            [-1.0, 0.75, 0.4],
            [0.3, 0.35, 0.7],
            [1.3, -0.15, 0.9],
            [0.9, 0.7, 1.2],
            [-0.5, -0.05, 0.7],
            [-1.2, -0.1, 0.8],
            [-1.2, -0.1, 1.2],
            [-0.0, -0.7, 1.2],
            [0.5, -0.75, 1.2],
        ]
    )
    self._t_total = 18  # s
    t = np.linspace(0, self._t_total, len(waypoints))
    self._des_pos_spline = CubicSpline(t, waypoints)
    self._des_vel_spline = self._des_pos_spline.derivative()

    self._tick = 0
    self._finished = False

Methods:

compute_control(obs, info=None)

Compute the next desired collective thrust and roll/pitch/yaw of the drone.

Parameters:

Name	Type	Description	Default
obs	dict[str, NDArray[floating]]	The current observation of the environment. See the environment's observation space for details.	required
info	dict | None	Optional additional information as a dictionary.	None

Returns:

Type	Description
NDArray[floating]	The orientation as roll, pitch, yaw angles, and the collective thrust
NDArray[floating]	[r_des, p_des, y_des, t_des] as a numpy array.

Source code in lsy_drone_racing/control/attitude_controller.py

def compute_control(
    self, obs: dict[str, NDArray[np.floating]], info: dict | None = None
) -> NDArray[np.floating]:
    """Compute the next desired collective thrust and roll/pitch/yaw of the drone.

    Args:
        obs: The current observation of the environment. See the environment's observation space
            for details.
        info: Optional additional information as a dictionary.

    Returns:
        The orientation as roll, pitch, yaw angles, and the collective thrust
        [r_des, p_des, y_des, t_des] as a numpy array.
    """
    t = min(self._tick / self._freq, self._t_total)
    if t >= self._t_total:  # Maximum duration reached
        self._finished = True

    des_pos = self._des_pos_spline(t)
    des_vel = self._des_vel_spline(t)
    des_yaw = 0.0

    # Calculate the deviations from the desired trajectory
    pos_error = des_pos - obs["pos"]
    vel_error = des_vel - obs["vel"]

    # Update integral error
    self.i_error += pos_error * (1 / self._freq)
    self.i_error = np.clip(self.i_error, -self.ki_range, self.ki_range)

    # Compute target thrust
    target_thrust = np.zeros(3)
    target_thrust += self.kp * pos_error
    target_thrust += self.ki * self.i_error
    target_thrust += self.kd * vel_error
    target_thrust[2] += self.drone_mass * self.g

    # Update z_axis to the current orientation of the drone
    z_axis = R.from_quat(obs["quat"]).as_matrix()[:, 2]

    # update current thrust
    thrust_desired = target_thrust.dot(z_axis)

    # update z_axis_desired
    z_axis_desired = target_thrust / np.linalg.norm(target_thrust)
    x_c_des = np.array([math.cos(des_yaw), math.sin(des_yaw), 0.0])
    y_axis_desired = np.cross(z_axis_desired, x_c_des)
    y_axis_desired /= np.linalg.norm(y_axis_desired)
    x_axis_desired = np.cross(y_axis_desired, z_axis_desired)

    R_desired = np.vstack([x_axis_desired, y_axis_desired, z_axis_desired]).T
    euler_desired = R.from_matrix(R_desired).as_euler("xyz", degrees=False)

    action = np.concatenate([euler_desired, [thrust_desired]], dtype=np.float32)

    return action

episode_callback()

Reset the internal state.

Source code in lsy_drone_racing/control/attitude_controller.py

def episode_callback(self):
    """Reset the internal state."""
    self.i_error[:] = 0
    self._tick = 0

episode_reset()

Reset the controller's internal state and models if necessary.

Source code in lsy_drone_racing/control/controller.py

def episode_reset(self):
    """Reset the controller's internal state and models if necessary."""

render_callback(sim)

Callback function called before the environment's rendering.

You can use this function to render additional information on the screen, such as the planned trajectory, the drone's target state, etc.

Source code in lsy_drone_racing/control/controller.py

def render_callback(self, sim: Sim):
    """Callback function called before the environment's rendering.

    You can use this function to render additional information on the screen, such as the
    planned trajectory, the drone's target state, etc.
    """

reset()

Reset internal variables if necessary.

Source code in lsy_drone_racing/control/controller.py

def reset(self):
    """Reset internal variables if necessary."""

step_callback(action, obs, reward, terminated, truncated, info)

Increment the tick counter.

Returns:

Type	Description
bool	True if the controller is finished, False otherwise.

Source code in lsy_drone_racing/control/attitude_controller.py

def step_callback(
    self,
    action: NDArray[np.floating],
    obs: dict[str, NDArray[np.floating]],
    reward: float,
    terminated: bool,
    truncated: bool,
    info: dict,
) -> bool:
    """Increment the tick counter.

    Returns:
        True if the controller is finished, False otherwise.
    """
    self._tick += 1
    return self._finished