This notebook solves QCQPs with quadratic constraints through AugmentedLagrangianOptimizer. It starts with a two-dimensional geometry example and finishes with quadrotor allocation under rotor bounds and a total-power limit.
import numpy as np
import gtsam
import plotly.graph_objects as go
from plotly.subplots import make_subplots
from IPython.display import display
from gtsam.symbol_shorthand import X
np.set_printoptions(precision=4, suppress=True)A 2D QCQP with active quadratic constraints¶
The target lies outside both the unit disk and the strip y^2 <= 0.25. The solution lands where the circular boundary and the upper strip boundary meet.
def augmented_lagrangian_params(max_iterations=200):
params = gtsam.AugmentedLagrangianParams()
params.maxIterations = max_iterations
params.absoluteViolationTolerance = 1e-8
params.relativeViolationTolerance = 1e-8
params.relativeCostTolerance = 1e-8
return params
key = X(0)
target = np.array([1.2, 0.8])
problem = gtsam.QcqpProblem()
problem.addCost(gtsam.QpCost(gtsam.HessianFactor(key, np.eye(2), target, float(target @ target))))
problem.addConstraint(gtsam.QuadraticConstraint.LessEqual(key, np.eye(2), 1.0))
problem.addConstraint(gtsam.QuadraticConstraint.LessEqual(key, np.diag([0.0, 1.0]), 0.25))
initial = gtsam.Values()
initial.insert(key, np.array([0.6, 0.2]))
result = gtsam.AugmentedLagrangianOptimizer(problem, initial, augmented_lagrangian_params()).optimize()
solution = result.atVector(key)
cost, equality_violation, inequality_violation = problem.evaluate(result)
print(f"target = {target}")
print(f"solution = {solution}")
print(f"x^2 + y^2 = {solution @ solution:.8f}")
print(f"y^2 = {solution[1] ** 2:.8f}")
print(f"violations: equality={equality_violation:.2e}, inequality={inequality_violation:.2e}")target = [1.2 0.8]
solution = [0.86602541 0.5 ]
x^2 + y^2 = 1.00000001
y^2 = 0.25000000
violations: equality=0.00e+00, inequality=8.94e-09
grid_x = np.linspace(-1.15, 1.35, 180)
grid_y = np.linspace(-0.85, 1.05, 180)
X_grid, Y_grid = np.meshgrid(grid_x, grid_y)
cost_surface = 0.5 * ((X_grid - target[0]) ** 2 + (Y_grid - target[1]) ** 2)
feasible = (X_grid ** 2 + Y_grid ** 2 <= 1.0) & (Y_grid ** 2 <= 0.25)
theta = np.linspace(0.0, 2.0 * np.pi, 240)
strip_x = np.linspace(-np.sqrt(0.75), np.sqrt(0.75), 100)
fig = go.Figure()
fig.add_trace(go.Contour(x=grid_x, y=grid_y, z=np.where(feasible, cost_surface, np.nan), colorscale='Viridis', contours=dict(showlabels=True), name='objective over feasible set'))
fig.add_trace(go.Scatter(x=np.cos(theta), y=np.sin(theta), mode='lines', line=dict(color='black', width=3), name='x² + y² = 1'))
fig.add_trace(go.Scatter(x=strip_x, y=0.5 * np.ones_like(strip_x), mode='lines', line=dict(color='crimson', width=3), name='y² = 0.25'))
fig.add_trace(go.Scatter(x=strip_x, y=-0.5 * np.ones_like(strip_x), mode='lines', line=dict(color='crimson', width=3, dash='dot'), name='lower strip boundary'))
fig.add_trace(go.Scatter(x=[target[0]], y=[target[1]], mode='markers+text', marker=dict(size=13, color='gold', line=dict(color='black', width=1)), text=['target'], textposition='top right', name='target'))
fig.add_trace(go.Scatter(x=[solution[0]], y=[solution[1]], mode='markers+text', marker=dict(size=15, color='white', line=dict(color='black', width=2)), text=['solution'], textposition='bottom right', name='solution'))
fig.update_layout(template='plotly_white', width=820, height=650, title='QCQP geometry: two active quadratic constraints', xaxis_title='x', yaxis_title='y', yaxis_scaleanchor='x', legend=dict(x=0.02, y=0.98))
display(fig)Loading...
Quadrotor allocation with total-power constraint¶
This QCQP uses the same wrench-tracking objective and rotor bounds as the QP notebook, then adds sum(rotor_i^2) <= 2.8. The initial point is feasible and close enough for the augmented Lagrangian method to converge to the active power boundary.
rotor_key = X(1)
arm_length = 0.25
yaw_moment_scale = 0.05
allocation = np.array([
[1.0, 1.0, 1.0, 1.0],
[arm_length, -arm_length, arm_length, -arm_length],
[arm_length, arm_length, -arm_length, -arm_length],
[yaw_moment_scale, -yaw_moment_scale, -yaw_moment_scale, yaw_moment_scale],
])
desired_wrench = np.array([3.4, 0.15, 0.10, 0.0])
sqrt_effort_weight = 1e-3
power_limit = 2.8
model4 = gtsam.noiseModel.Unit.Create(4)
quadrotor_qcqp = gtsam.QcqpProblem()
quadrotor_qcqp.addCost(gtsam.QpCost(gtsam.JacobianFactor(rotor_key, allocation, desired_wrench, model4)))
quadrotor_qcqp.addCost(gtsam.QpCost(gtsam.JacobianFactor(rotor_key, sqrt_effort_weight * np.eye(4), np.zeros(4), model4)))
quadrotor_qcqp.addConstraint(gtsam.LinearConstraint.LessEqual(gtsam.JacobianFactor(rotor_key, np.eye(4), np.ones(4), model4)))
quadrotor_qcqp.addConstraint(gtsam.LinearConstraint.GreaterEqual(gtsam.JacobianFactor(rotor_key, np.eye(4), np.zeros(4), model4)))
quadrotor_qcqp.addConstraint(gtsam.QuadraticConstraint.LessEqual(rotor_key, np.eye(4), power_limit))
initial_rotors = np.array([1.0, 0.8, 0.85, np.sqrt(power_limit - 1.0 - 0.8 ** 2 - 0.85 ** 2)])
initial = gtsam.Values()
initial.insert(rotor_key, initial_rotors)
params = gtsam.AugmentedLagrangianParams()
params.updatePolicy = gtsam.AugmentedLagrangianUpdatePolicy.Aggressive
params.maxIterations = 2000
params.absoluteViolationTolerance = 1e-12
params.relativeViolationTolerance = 1e-12
params.relativeCostTolerance = 1e-14
params.absoluteCostTolerance = 1e-14
params.initialMuIneq = 100.0
params.muIneqIncreaseRate = 10.0
params.maxDualStepSizeIneq = 100.0
params.muIncreaseThreshold = 0.01
quad_result = gtsam.AugmentedLagrangianOptimizer(quadrotor_qcqp, initial, params).optimize()
rotors = quad_result.atVector(rotor_key)
achieved_wrench = allocation @ rotors
residual = achieved_wrench - desired_wrench
total_power = float(rotors @ rotors)
quad_cost, _, quad_ineq = quadrotor_qcqp.evaluate(quad_result)
print(f"initial rotor thrusts = {initial_rotors}")
print(f"solution rotor thrusts = {rotors}")
print(f"total power = {total_power:.8f} / {power_limit}")
print(f"achieved wrench = {achieved_wrench}")
print(f"wrench residual = {residual}")
print(f"objective = {quad_cost:.6e}, inequality violation = {quad_ineq:.2e}")initial rotor thrusts = [1. 0.8 0.85 0.66143783]
solution rotor thrusts = [0.99999829 0.79330414 0.8628766 0.65277555]
total power = 2.80000000 / 2.8
achieved wrench = [ 3.30895459e+00 1.04198800e-01 6.94125698e-02 -1.70344889e-04]
wrench residual = [-0.09104541 -0.0458012 -0.03058743 -0.00017034]
objective = 5.662718e-03, inequality violation = 4.34e-12
rotor_names = [f'rotor {i+1}' for i in range(4)]
wrench_names = ['thrust', 'roll', 'pitch', 'yaw']
upper_margin = 1.0 - rotors
lower_margin = rotors
power_margin = power_limit - total_power
active_upper = np.isclose(rotors, 1.0, atol=5e-5)
rotor_colors = np.where(active_upper, 'crimson', 'steelblue')
constraint_rows = [
['upper bound min margin', f'{upper_margin.min():.3e}', 'active' if active_upper.any() else 'inactive'],
['lower bound min margin', f'{lower_margin.min():.3e}', 'inactive'],
['total power margin', f'{power_margin:.3e}', 'active'],
]
fig = make_subplots(
rows=2, cols=2,
specs=[[{'type': 'bar'}, {'type': 'bar'}], [{'type': 'indicator'}, {'type': 'table'}]],
subplot_titles=('Rotor thrusts', 'Desired vs achieved wrench', 'Total power', 'Constraint activity'),
)
fig.add_trace(go.Bar(x=rotor_names, y=rotors, marker_color=rotor_colors, name='rotor thrust'), row=1, col=1)
fig.add_trace(go.Scatter(x=rotor_names, y=np.ones(4), mode='lines', line=dict(color='black', dash='dash'), name='upper bound'), row=1, col=1)
fig.add_trace(go.Bar(x=wrench_names, y=desired_wrench, marker_color='lightgray', name='desired'), row=1, col=2)
fig.add_trace(go.Bar(x=wrench_names, y=achieved_wrench, marker_color='seagreen', name='achieved'), row=1, col=2)
fig.add_trace(go.Indicator(mode='gauge+number+delta', value=total_power, delta={'reference': power_limit}, gauge={'axis': {'range': [0, power_limit]}, 'bar': {'color': 'crimson'}, 'threshold': {'line': {'color': 'black', 'width': 3}, 'thickness': 0.8, 'value': power_limit}}, title={'text': 'sum rotor²'}), row=2, col=1)
fig.add_trace(go.Table(header=dict(values=['constraint', 'margin', 'status'], fill_color='aliceblue'), cells=dict(values=list(zip(*constraint_rows)), fill_color='white')), row=2, col=2)
fig.update_yaxes(range=[0, 1.08], row=1, col=1)
fig.update_layout(template='plotly_white', width=1050, height=760, title='Quadrotor QCQP: rotor bound and total-power constraint shape the allocation', barmode='group', legend=dict(orientation='h', y=-0.1))
display(fig)Loading...