This notebook starts with a small geometric QP, then reproduces the quadrotor allocation QP from timing/timeActiveSetSolverComparison.cpp and visualizes active rotor bounds and wrench residuals with Plotly.
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 QP with equality and active inequality¶
The unconstrained target lies to the right of the feasible segment. The equality x + y = 1 restricts the solution to a line, and x <= 0.35 becomes active.
key = X(0)
target = np.array([0.85, 0.55])
model1 = gtsam.noiseModel.Unit.Create(1)
model2 = gtsam.noiseModel.Unit.Create(2)
problem = gtsam.QpProblem()
problem.addCost(gtsam.HessianFactor(key, np.eye(2), target, float(target @ target)))
problem.addConstraint(
gtsam.LinearConstraint.Equal(
gtsam.JacobianFactor(key, np.array([[1.0, 1.0]]), np.array([1.0]), model1)
)
)
problem.addConstraint(
gtsam.LinearConstraint.LessEqual(
gtsam.JacobianFactor(key, np.array([[1.0, 0.0]]), np.array([0.35]), model1)
)
)
problem.addConstraint(
gtsam.LinearConstraint.GreaterEqual(
gtsam.JacobianFactor(key, np.eye(2), np.zeros(2), model2)
)
)
initial = gtsam.Values()
initial.insert(key, np.array([0.2, 0.8]))
result = problem.optimize(initial, gtsam.QpSolverType.Sparse)
solution = result.atVector(key)
cost, equality_violation, inequality_violation = problem.evaluate(result)
print(f"target = {target}")
print(f"solution = {solution}")
print(f"objective = {cost:.6f}")
print(f"violations: equality={equality_violation:.2e}, inequality={inequality_violation:.2e}")target = [0.85 0.55]
solution = [0.35 0.65]
objective = 0.130000
violations: equality=0.00e+00, inequality=0.00e+00
grid_x = np.linspace(-0.05, 1.1, 180)
grid_y = np.linspace(-0.05, 1.1, 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)
line_x = np.linspace(-0.05, 1.05, 120)
line_y = 1.0 - line_x
feasible_x = np.linspace(0.0, 0.35, 60)
feasible_y = 1.0 - feasible_x
fig = go.Figure()
fig.add_trace(go.Contour(x=grid_x, y=grid_y, z=cost_surface, colorscale='Cividis', opacity=0.7, contours=dict(showlabels=True), name='quadratic objective'))
fig.add_trace(go.Scatter(x=line_x, y=line_y, mode='lines', line=dict(color='black', dash='dash'), name='x + y = 1'))
fig.add_trace(go.Scatter(x=feasible_x, y=feasible_y, mode='lines', line=dict(color='seagreen', width=7), name='feasible segment'))
fig.add_trace(go.Scatter(x=[0.35, 0.35], y=[-0.05, 1.1], mode='lines', line=dict(color='crimson', width=3), name='x <= 0.35'))
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='bottom right', name='unconstrained 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='top center', name='solution'))
fig.update_layout(template='plotly_white', width=800, height=620, title='QP geometry: projection onto constrained segment', xaxis_title='x', yaxis_title='y', yaxis_scaleanchor='x', legend=dict(x=0.02, y=0.98))
display(fig)Loading...
Quadrotor allocation QP¶
This is the same allocation model used in timing/timeActiveSetSolverComparison.cpp: four rotor thrusts are chosen to track a desired wrench while staying inside [0, 1] bounds.
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
model4 = gtsam.noiseModel.Unit.Create(4)
quadrotor_qp = gtsam.QpProblem()
quadrotor_qp.addCost(gtsam.JacobianFactor(rotor_key, allocation, desired_wrench, model4))
quadrotor_qp.addCost(gtsam.JacobianFactor(rotor_key, sqrt_effort_weight * np.eye(4), np.zeros(4), model4))
quadrotor_qp.addConstraint(
gtsam.LinearConstraint.LessEqual(
gtsam.JacobianFactor(rotor_key, np.eye(4), np.ones(4), model4)
)
)
quadrotor_qp.addConstraint(
gtsam.LinearConstraint.GreaterEqual(
gtsam.JacobianFactor(rotor_key, np.eye(4), np.zeros(4), model4)
)
)
initial = gtsam.Values()
initial.insert(rotor_key, 0.5 * np.ones(4))
quad_result = quadrotor_qp.optimize(initial, gtsam.QpSolverType.Sparse)
rotors = quad_result.atVector(rotor_key)
achieved_wrench = allocation @ rotors
residual = achieved_wrench - desired_wrench
quad_cost, _, quad_ineq = quadrotor_qp.evaluate(quad_result)
print(f"rotor thrusts = {rotors}")
print(f"achieved wrench = {achieved_wrench}")
print(f"wrench residual = {residual}")
print(f"objective = {quad_cost:.6e}, inequality violation = {quad_ineq:.2e}")rotor thrusts = [1. 0.89214598 0.99214558 0.51478374]
achieved wrench = [ 3.39907529 0.14630397 0.09630417 -0.01847539]
wrench residual = [-0.00092471 -0.00369603 -0.00369583 -0.01847539]
objective = 1.862801e-04, inequality violation = 0.00e+00
rotor_names = [f'rotor {i+1}' for i in range(4)]
wrench_names = ['thrust', 'roll', 'pitch', 'yaw']
active_upper = np.isclose(rotors, 1.0, atol=1e-5)
rotor_colors = np.where(active_upper, 'crimson', 'steelblue')
fig = make_subplots(rows=2, cols=2, subplot_titles=('Rotor thrusts', 'Desired vs achieved wrench', 'Wrench residual', 'Distance to upper bound'))
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.Bar(x=wrench_names, y=residual, marker_color='darkorange', name='residual'), row=2, col=1)
fig.add_trace(go.Bar(x=rotor_names, y=1.0 - rotors, marker_color='mediumpurple', name='upper margin'), row=2, col=2)
fig.update_yaxes(range=[0, 1.08], row=1, col=1)
fig.update_layout(template='plotly_white', width=1000, height=720, title='Quadrotor QP: one rotor saturates to satisfy the wrench request', barmode='group', legend=dict(orientation='h', y=-0.12))
display(fig)Loading...