|
| 1 | +"""This module implements a ThrustController for quadrotor control. |
| 2 | +
|
| 3 | +It utilizes the collective thrust interface for drone control to compute control commands based on |
| 4 | +current state observations and desired waypoints. The attitude control is handled by computing a |
| 5 | +PID control law for position tracking, incorporating gravity compensation in thrust calculations. |
| 6 | +
|
| 7 | +The waypoints are generated using cubic spline interpolation from a set of predefined waypoints. |
| 8 | +Note that the trajectory uses pre-defined waypoints instead of dynamically generating a good path. |
| 9 | +""" |
| 10 | + |
| 11 | +from __future__ import annotations # Python 3.10 type hints |
| 12 | + |
| 13 | +import math |
| 14 | +from typing import TYPE_CHECKING |
| 15 | + |
| 16 | +import numpy as np |
| 17 | +import pybullet as p |
| 18 | +from scipy.interpolate import CubicSpline |
| 19 | +from scipy.spatial.transform import Rotation as R |
| 20 | + |
| 21 | +from lsy_drone_racing.control import BaseController |
| 22 | + |
| 23 | +if TYPE_CHECKING: |
| 24 | + from numpy.typing import NDArray |
| 25 | + |
| 26 | + |
| 27 | +class ThrustController(BaseController): |
| 28 | + """Example of a controller using the collective thrust and attitude interface. |
| 29 | +
|
| 30 | + Modified from https://github.com/utiasDSL/crazyswarm-import/blob/ad2f7ea987f458a504248a1754b124ba39fc2f21/ros_ws/src/crazyswarm/scripts/position_ctl_m.py |
| 31 | + """ |
| 32 | + |
| 33 | + def __init__(self, initial_obs: dict[str, NDArray[np.floating]], initial_info: dict): |
| 34 | + """Initialization of the controller. |
| 35 | +
|
| 36 | + Args: |
| 37 | + initial_obs: The initial observation of the environment's state. See the environment's |
| 38 | + observation space for details. |
| 39 | + initial_info: Additional environment information from the reset. |
| 40 | + """ |
| 41 | + super().__init__(initial_obs, initial_info) |
| 42 | + self.low_level_ctrl_freq = initial_info["low_level_ctrl_freq"] |
| 43 | + self.drone_mass = initial_info["drone_mass"] |
| 44 | + self.kp = np.array([0.4, 0.4, 1.25]) |
| 45 | + self.ki = np.array([0.05, 0.05, 0.05]) |
| 46 | + self.kd = np.array([0.2, 0.2, 0.4]) |
| 47 | + self.ki_range = np.array([2.0, 2.0, 0.4]) |
| 48 | + self.i_error = np.zeros(3) |
| 49 | + self.g = 9.81 |
| 50 | + self._tick = 0 |
| 51 | + |
| 52 | + # Same waypoints as in the trajectory controller. Determined by trial and error. |
| 53 | + waypoints = np.array( |
| 54 | + [ |
| 55 | + [1.0, 1.0, 0.05], |
| 56 | + [1.0, 1.0, 1.0], |
| 57 | + [1.0, 1.0, 1.0], |
| 58 | + [1.0, 1.0, 1.0], |
| 59 | + [1.0, 1.0, 1.0], |
| 60 | + ] |
| 61 | + ) |
| 62 | + # Scale trajectory between 0 and 1 |
| 63 | + ts = np.linspace(0, 1, np.shape(waypoints)[0]) |
| 64 | + cs_x = CubicSpline(ts, waypoints[:, 0]) |
| 65 | + cs_y = CubicSpline(ts, waypoints[:, 1]) |
| 66 | + cs_z = CubicSpline(ts, waypoints[:, 2]) |
| 67 | + |
| 68 | + des_completion_time = 30 |
| 69 | + ts = np.linspace(0, 1, int(initial_info["env_freq"] * des_completion_time)) |
| 70 | + |
| 71 | + self.x_des = cs_x(ts) |
| 72 | + self.y_des = cs_y(ts) |
| 73 | + self.z_des = cs_z(ts) |
| 74 | + |
| 75 | + try: |
| 76 | + # Draw interpolated Trajectory. Limit segments to avoid excessive drawings |
| 77 | + stride = max(1, len(self.x_des) // 100) |
| 78 | + trajectory = np.vstack([self.x_des, self.y_des, self.z_des])[..., ::stride].T |
| 79 | + for i in range(len(trajectory) - 1): |
| 80 | + p.addUserDebugLine( |
| 81 | + trajectory[i], |
| 82 | + trajectory[i + 1], |
| 83 | + lineColorRGB=[1, 0, 0], |
| 84 | + lineWidth=2, |
| 85 | + lifeTime=0, |
| 86 | + physicsClientId=0, |
| 87 | + ) |
| 88 | + except p.error: |
| 89 | + ... # Ignore pybullet errors if not running in the pybullet GUI |
| 90 | + |
| 91 | + def compute_control( |
| 92 | + self, obs: dict[str, NDArray[np.floating]], info: dict | None = None |
| 93 | + ) -> NDArray[np.floating]: |
| 94 | + """Compute the next desired collective thrust and roll/pitch/yaw of the drone. |
| 95 | +
|
| 96 | + Args: |
| 97 | + obs: The current observation of the environment. See the environment's observation space |
| 98 | + for details. |
| 99 | + info: Optional additional information as a dictionary. |
| 100 | +
|
| 101 | + Returns: |
| 102 | + The collective thrust and orientation [t_des, r_des, p_des, y_des] as a numpy array. |
| 103 | + """ |
| 104 | + i = min(self._tick, len(self.x_des) - 1) |
| 105 | + des_pos = np.array([self.x_des[i], self.y_des[i], self.z_des[i]]) |
| 106 | + des_vel = np.zeros(3) |
| 107 | + des_yaw = 0.0 |
| 108 | + |
| 109 | + # Calculate the deviations from the desired trajectory |
| 110 | + pos_error = des_pos - obs["pos"] |
| 111 | + vel_error = des_vel - obs["vel"] |
| 112 | + |
| 113 | + # Update integral error |
| 114 | + self.i_error += pos_error * (1 / self.low_level_ctrl_freq) |
| 115 | + self.i_error = np.clip(self.i_error, -self.ki_range, self.ki_range) |
| 116 | + |
| 117 | + # Compute target thrust |
| 118 | + target_thrust = np.zeros(3) |
| 119 | + target_thrust += self.kp * pos_error |
| 120 | + target_thrust += self.ki * self.i_error |
| 121 | + target_thrust += self.kd * vel_error |
| 122 | + # target_thrust += params.quad.m * desired_acc |
| 123 | + target_thrust[2] += self.drone_mass * self.g |
| 124 | + |
| 125 | + # Update z_axis to the current orientation of the drone |
| 126 | + z_axis = R.from_euler("xyz", obs["rpy"]).as_matrix()[:, 2] |
| 127 | + |
| 128 | + # update current thrust |
| 129 | + thrust_desired = target_thrust.dot(z_axis) |
| 130 | + thrust_desired = max(thrust_desired, 0.3 * self.drone_mass * self.g) |
| 131 | + thrust_desired = min(thrust_desired, 1.8 * self.drone_mass * self.g) |
| 132 | + |
| 133 | + # update z_axis_desired |
| 134 | + z_axis_desired = target_thrust / np.linalg.norm(target_thrust) |
| 135 | + x_c_des = np.array([math.cos(des_yaw), math.sin(des_yaw), 0.0]) |
| 136 | + y_axis_desired = np.cross(z_axis_desired, x_c_des) |
| 137 | + y_axis_desired /= np.linalg.norm(y_axis_desired) |
| 138 | + x_axis_desired = np.cross(y_axis_desired, z_axis_desired) |
| 139 | + |
| 140 | + R_desired = np.vstack([x_axis_desired, y_axis_desired, z_axis_desired]).T |
| 141 | + euler_desired = R.from_matrix(R_desired).as_euler("xyz", degrees=False) |
| 142 | + thrust_desired, euler_desired |
| 143 | + return np.concatenate([[thrust_desired], euler_desired]) |
| 144 | + |
| 145 | + def step_callback( |
| 146 | + self, |
| 147 | + action: NDArray[np.floating], |
| 148 | + obs: dict[str, NDArray[np.floating]], |
| 149 | + reward: float, |
| 150 | + terminated: bool, |
| 151 | + truncated: bool, |
| 152 | + info: dict, |
| 153 | + ): |
| 154 | + """Increment the tick counter.""" |
| 155 | + self._tick += 1 |
| 156 | + |
| 157 | + def episode_callback(self): |
| 158 | + """Reset the integral error.""" |
| 159 | + self.i_error[:] = 0 |
| 160 | + self._tick = 0 |
0 commit comments