Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions .github/workflows/ci.yml
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@ name: CI

on:
push:
branches: [main]
branches: [main, "feat/**", "fix/**"]
pull_request:
branches: [main]
branches: [main, "feat/**", "fix/**"]

permissions:
contents: read
Expand Down
137 changes: 107 additions & 30 deletions docs/AUTONOMOUS_NAVIGATION.md

Large diffs are not rendered by default.

2 changes: 2 additions & 0 deletions pyproject.toml
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ dependencies = [
"rich>=13.0.0",
"pydantic>=2.0.0",
"numpy>=1.24.0",
"scipy>=1.10.0",
]

[project.optional-dependencies]
Expand Down Expand Up @@ -65,6 +66,7 @@ all = [
"streamlit>=1.30.0",
"plotly>=5.18.0",
"matplotlib>=3.7.0",
"scipy>=1.10.0",
"pytest>=7.4.0",
"pytest-cov>=4.1.0",
"ruff>=0.1.0",
Expand Down
207 changes: 168 additions & 39 deletions src/adaptive_rl/controllers/lqr.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,10 @@ def __init__(
q_rate: float = 2.0,
r_thrust: float = 1.0,
r_torque: float = 5.0,
bounds: Tuple[float, float, float] = (30.0, 30.0, 15.0),
max_velocity: float = 10.0,
max_angular_velocity: float = 10.0,
allow_fallback: bool = False,
) -> None:
self.mass = float(mass)
self.gravity = float(gravity)
Expand All @@ -42,6 +46,10 @@ def __init__(
self.max_thrust_per_motor = float(max_thrust_per_motor)
self.c_tau = float(torque_to_thrust_ratio)
self.dt = float(dt)
self.bounds = tuple(float(b) for b in bounds)
self.max_velocity = float(max_velocity)
self.max_angular_velocity = float(max_angular_velocity)
self.allow_fallback = bool(allow_fallback)

self.hover_thrust_total = self.mass * self.gravity
self.hover_thrust_per_motor = self.hover_thrust_total / 4.0
Expand Down Expand Up @@ -69,10 +77,80 @@ def __init__(
# Controls: [Delta_T, tau_x, tau_y, tau_z]
self.R = np.diag([r_thrust, r_torque, r_torque, r_torque * 1.5])

self.solver_status: str = "INITIALIZING"
self.is_are_solution: bool = False

# Compute optimal gain matrix K
self.K = self._compute_lqr_gain()
self.target_position = np.array([0.0, 0.0, 2.0], dtype=np.float64)

@staticmethod
def get_allocation_matrix(d: float, c_tau: float) -> np.ndarray:
"""Construct the physical forward allocation matrix M (4x4) for an X-quadrotor.

Maps individual rotor thrusts [f1, f2, f3, f4]^T to [T, tau_x, tau_y, tau_z]^T:
T = f1 + f2 + f3 + f4
tau_x = d * (-f1 + f2 + f3 - f4)
tau_y = d * (-f1 - f2 + f3 + f4)
tau_z = c_tau * (+f1 - f2 + f3 - f4)
"""
return np.array(
[
[1.0, 1.0, 1.0, 1.0],
[-d, d, d, -d],
[-d, -d, d, d],
[c_tau, -c_tau, c_tau, -c_tau],
],
dtype=np.float64,
)

@staticmethod
def get_allocation_matrix_inverse(d: float, c_tau: float) -> np.ndarray:
"""Construct the exact analytical inverse allocation matrix M^{-1} (4x4).

Maps virtual control wrench [T, tau_x, tau_y, tau_z]^T to [f1, f2, f3, f4]^T:
f1 = T/4 - tau_x/(4d) - tau_y/(4d) + tau_z/(4*c_tau)
f2 = T/4 + tau_x/(4d) - tau_y/(4d) - tau_z/(4*c_tau)
f3 = T/4 + tau_x/(4d) + tau_y/(4d) + tau_z/(4*c_tau)
f4 = T/4 - tau_x/(4d) + tau_y/(4d) - tau_z/(4*c_tau)
"""
if d <= 1e-9:
raise ValueError(f"Arm diagonal offset d must be positive, got {d}")
if c_tau <= 1e-12:
raise ValueError(f"Torque constant c_tau must be positive, got {c_tau}")

return np.array(
[
[0.25, -0.25 / d, -0.25 / d, 0.25 / c_tau],
[0.25, 0.25 / d, -0.25 / d, -0.25 / c_tau],
[0.25, 0.25 / d, 0.25 / d, 0.25 / c_tau],
[0.25, -0.25 / d, 0.25 / d, -0.25 / c_tau],
],
dtype=np.float64,
)

def allocate_thrusts(
self,
total_thrust: float,
tau_x: float,
tau_y: float,
tau_z: float,
) -> np.ndarray:
"""Invert the physical X-quadrotor allocation matrix analytically."""
d = self.d
c = self.c_tau
t_term = total_thrust * 0.25
tx_term = tau_x / (4.0 * d)
ty_term = tau_y / (4.0 * d)
tz_term = tau_z / (4.0 * c) if c > 1e-9 else 0.0

f1 = t_term - tx_term - ty_term + tz_term
f2 = t_term + tx_term - ty_term - tz_term
f3 = t_term + tx_term + ty_term + tz_term
f4 = t_term - tx_term + ty_term - tz_term

return np.array([f1, f2, f3, f4], dtype=np.float64)

def _compute_lqr_gain(self) -> np.ndarray:
"""Formulate linearized state space (A, B) and solve the continuous ARE."""
A = np.zeros((12, 12), dtype=np.float64)
Expand Down Expand Up @@ -104,9 +182,21 @@ def _compute_lqr_gain(self) -> np.ndarray:
P = solve_continuous_are(A, B, self.Q, self.R)
R_inv = np.linalg.inv(self.R)
K = R_inv @ (B.T @ P)
self.solver_status = "ARE_SOLVED"
self.is_are_solution = True
return np.asarray(K, dtype=np.float64)
except Exception:
# Analytical decoupled LQR gain approximation if scipy is unavailable
except Exception as err:
if not self.allow_fallback:
raise RuntimeError(
f"Continuous Algebraic Riccati Equation solver failed: {err}. "
"Exact LQR control requires scipy.linalg.solve_continuous_are. "
"Do not silently call an approximate gain matrix an ARE-derived LQR solution; "
"set allow_fallback=True only if an explicit heuristic approximation is acceptable."
) from err

self.solver_status = "UNRESOLVED_APPROXIMATION"
self.is_are_solution = False
# Analytical decoupled gain approximation
K_approx = np.zeros((4, 12), dtype=np.float64)
# z altitude channel: [z, vz] -> Delta_T
K_approx[0, 2] = math.sqrt(self.Q[2, 2] / self.R[0, 0])
Expand Down Expand Up @@ -136,33 +226,84 @@ def reset(self) -> None:

def compute_action(
self,
state: DroneState6DOF,
state_or_obs: DroneState6DOF | np.ndarray,
target_position: Optional[np.ndarray] = None,
target_velocity: Optional[np.ndarray] = None,
target_yaw: float = 0.0,
dt: float = 0.05,
) -> Tuple[np.ndarray, Dict[str, Any]]:
"""Compute optimal control action for the 6-DOF quadrotor.

Accepts either a physical DroneState6DOF object, a 36-D normalized Gymnasium
observation from Drone6DOFEnv, or an unnormalized 13-D state vector.

Returns:
(action_in_minus1_plus1, debug_info_dict)
"""
target_pos = (
np.asarray(target_position, dtype=np.float64)
if target_position is not None
else self.target_position
)
if isinstance(state_or_obs, DroneState6DOF):
pos = state_or_obs.position.copy()
vel = state_or_obs.velocity.copy()
roll, pitch, yaw = quaternion_to_euler_angles(state_or_obs.quaternion)
omega = state_or_obs.angular_velocity.copy()
target_pos = (
np.asarray(target_position, dtype=np.float64).copy()
if target_position is not None
else self.target_position.copy()
)
else:
arr = np.asarray(state_or_obs, dtype=np.float64)
if len(arr) == 36:
bx, by, bz = self.bounds
pos = np.array([arr[0] * bx, arr[1] * by, arr[2] * bz], dtype=np.float64)
vel = np.array(
[
arr[3] * self.max_velocity,
arr[4] * self.max_velocity,
arr[5] * self.max_velocity,
],
dtype=np.float64,
)
quat = np.array([arr[6], arr[7], arr[8], arr[9]], dtype=np.float64)
roll, pitch, yaw = quaternion_to_euler_angles(quat)
omega = np.array(
[
arr[10] * self.max_angular_velocity,
arr[11] * self.max_angular_velocity,
arr[12] * self.max_angular_velocity,
],
dtype=np.float64,
)
if target_position is not None:
target_pos = np.asarray(target_position, dtype=np.float64).copy()
else:
# De-normalize target from observation norm_goal indices 13:16
target_pos = np.array(
[arr[13] * bx, arr[14] * by, arr[15] * bz], dtype=np.float64
)
elif len(arr) == 13:
pos = arr[0:3].copy()
vel = arr[3:6].copy()
quat = arr[6:10].copy()
roll, pitch, yaw = quaternion_to_euler_angles(quat)
omega = arr[10:13].copy()
target_pos = (
np.asarray(target_position, dtype=np.float64).copy()
if target_position is not None
else self.target_position.copy()
)
else:
raise ValueError(
f"Unsupported state/obs representation with shape {arr.shape}. "
"Expected DroneState6DOF, 36-D normalized Drone6DOFEnv observation, "
"or 13-D unnormalized physical state vector."
)

target_vel = (
np.asarray(target_velocity, dtype=np.float64)
if target_velocity is not None
else np.zeros(3, dtype=np.float64)
)

pos = state.position
vel = state.velocity
roll, pitch, yaw = quaternion_to_euler_angles(state.quaternion)
omega = state.angular_velocity

# State error vector (12,)
# delta_x = x - x_ref
delta_x = np.array(
Expand Down Expand Up @@ -196,15 +337,8 @@ def compute_action(
total_thrust = max(0.0, min(4.0 * self.max_thrust_per_motor, total_thrust))

# Exact analytical mixer inversion for X-quadrotor
d = self.d
c_tau = self.c_tau
t1 = 0.25 * total_thrust - 0.25 * tau_x / d - 0.25 * tau_y / d - 0.25 * tau_z / c_tau
t2 = 0.25 * total_thrust - 0.25 * tau_x / d + 0.25 * tau_y / d + 0.25 * tau_z / c_tau
t3 = 0.25 * total_thrust + 0.25 * tau_x / d + 0.25 * tau_y / d - 0.25 * tau_z / c_tau
t4 = 0.25 * total_thrust + 0.25 * tau_x / d - 0.25 * tau_y / d + 0.25 * tau_z / c_tau

thrusts = np.array([t1, t2, t3, t4], dtype=np.float64)
thrusts_clamped = np.clip(thrusts, 0.0, self.max_thrust_per_motor)
raw_thrusts = self.allocate_thrusts(total_thrust, tau_x, tau_y, tau_z)
thrusts_clamped = np.clip(raw_thrusts, 0.0, self.max_thrust_per_motor)

# Normalize to Gym action space [-1, 1]
action = np.clip(
Expand All @@ -218,29 +352,24 @@ def compute_action(
"tau_y": tau_y,
"tau_z": tau_z,
"motor_thrusts": thrusts_clamped,
"raw_motor_thrusts": raw_thrusts,
"position_error": float(np.linalg.norm(target_pos - pos)),
"solver_status": self.solver_status,
"is_are_solution": self.is_are_solution,
}
return action, info

def predict(
self,
obs: np.ndarray,
observation: np.ndarray | DroneState6DOF,
state: Optional[Any] = None,
episode_start: Optional[Any] = None,
deterministic: bool = True,
) -> Tuple[np.ndarray, Optional[Dict[str, Any]]]:
"""Gym-compatible interface matching RL policy predict signature."""
from adaptive_rl.environments.drone_6dof import DroneState6DOF

pos = obs[0:3]
vel = obs[3:6]
quat = obs[6:10]
omega = obs[10:13]
target = obs[13:16]

state = DroneState6DOF(
position=np.array(pos, dtype=np.float64),
velocity=np.array(vel, dtype=np.float64),
quaternion=np.array(quat, dtype=np.float64),
angular_velocity=np.array(omega, dtype=np.float64),
)
action, info = self.compute_action(state, target_position=target)
"""Gym-compatible interface matching RL policy predict signature.

Correctly decodes normalized 36-D observation from Drone6DOFEnv or accepts DroneState6DOF.
Rejects unsupported formats with an explicit ValueError.
"""
action, info = self.compute_action(observation)
return action, info
Loading
Loading