control.state_controller

lsy_drone_racing.control.state_controller

Controller that follows a pre-defined trajectory.

It uses a cubic spline interpolation to generate a smooth trajectory through a series of waypoints. At each time step, the controller computes the next desired position by evaluating the spline.

.. note:: The waypoints are hard-coded in the controller for demonstration purposes. In practice, you would need to generate the splines adaptively based on the track layout, and recompute the trajectory if you receive updated gate and obstacle poses.

Classes

StateController(obs, info, config)

Bases: Controller

State controller following a pre-defined trajectory.

Initialization of the 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	The initial environment information from the reset.	required
config	dict	The race configuration. See the config files for details. Contains additional information such as disturbance configurations, randomizations, etc.	required

Source code in lsy_drone_racing/control/state_controller.py

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

    Args:
        obs: The initial observation of the environment's state. See the environment's
            observation space for details.
        info: The initial environment information from the reset.
        config: The race configuration. See the config files for details. Contains additional
            information such as disturbance configurations, randomizations, etc.
    """
    super().__init__(obs, info, config)
    self._freq = config.env.freq

    # Same waypoints as in the attitude 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.85, 0.85, 1.2],
            [-0.5, -0.05, 0.7],
            [-1.2, -0.2, 0.8],
            [-1.2, -0.2, 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._tick = 0
    self._finished = False

Methods:

compute_control(obs, info=None)

Compute the next desired state 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 drone state [x, y, z, vx, vy, vz, ax, ay, az, yaw, rrate, prate, yrate] as a numpy array.

Source code in lsy_drone_racing/control/state_controller.py

def compute_control(
    self, obs: dict[str, NDArray[np.floating]], info: dict | None = None
) -> NDArray[np.floating]:
    """Compute the next desired state 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 drone state [x, y, z, vx, vy, vz, ax, ay, az, yaw, rrate, prate, yrate] 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)
    action = np.concatenate((des_pos, np.zeros(10)), dtype=np.float32)
    return action

episode_callback()

Reset the internal state.

Source code in lsy_drone_racing/control/state_controller.py

def episode_callback(self):
    """Reset the internal state."""
    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)

Visualize the desired trajectory and the current setpoint.

Source code in lsy_drone_racing/control/state_controller.py

def render_callback(self, sim: Sim):
    """Visualize the desired trajectory and the current setpoint."""
    setpoint = self._des_pos_spline(self._tick / self._freq).reshape(1, -1)
    draw_points(sim, setpoint, rgba=(1.0, 0.0, 0.0, 1.0), size=0.02)
    trajectory = self._des_pos_spline(np.linspace(0, self._t_total, 100))
    draw_line(sim, trajectory, rgba=(0.0, 1.0, 0.0, 1.0))

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 time step counter.

Returns:

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

Source code in lsy_drone_racing/control/state_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 time step counter.

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