For instructions on how to run these tutorial notebooks, please see the index.
Often cases after we solve an optimization problem, we want to tweak its constraints and costs slightly, and then resolve the updated problem. One use case is in model predictive control, where in each time instance we solve a new optimization problem, whose constraints/costs are just slightly different from the one in the previous time instance.
Instead of constructing a new MathematicalProgram object, we could update the constraints/costs in the old MathematicalProgram object, and then solve the updated problem. To do so, many constraints/costs object provide an "update" function. In this tutorial we show how to update certain types of constraints/costs
LinearCost¶For a linear cost $a^Tx + b$, we could call LinearCost.UpdateCoefficients() function to update the linear coefficient a vector or the constant term b
from pydrake.solvers import MathematicalProgram, Solve
import numpy as np
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2)
cost1 = prog.AddLinearCost(2*x[0] + 3 * x[1] + 2)
print(f"original cost: {cost1}")
prog.AddBoundingBoxConstraint(-1, 1, x)
result = Solve(prog)
print(f"optimal solution: {result.GetSolution(x)}")
print(f"original optimal cost: {result.get_optimal_cost()}")
# Now update the cost to 3x[0] - 4x[1] + 5
cost1.evaluator().UpdateCoefficients(new_a=[3, -4], new_b=5)
print(f"updated cost: {cost1}")
# Solve the optimization problem again with the updated cost.
result = Solve(prog)
print(f"updated optimal solution: {result.GetSolution(x)}")
print(f"updated optimal cost: {result.get_optimal_cost()}")
original cost: LinearCost (2 + 2 * x(0) + 3 * x(1)) optimal solution: [-1. -1.] original optimal cost: -3.0 updated cost: LinearCost (5 + 3 * x(0) - 4 * x(1)) updated optimal solution: [-1. 1.] updated optimal cost: -2.0
QuadraticCost¶For a quadratic cost in the form $0.5x^TQx + b'x + c$, we can also call QuadraticCost.UpdateCoefficients to update its $Q, b, c$ terms
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2)
cost1 = prog.AddQuadraticCost(x[0]**2 + 2 * x[1]**2 + x[0]*x[1] + 3*x[0] + 5)
print(f" original cost: {cost1}")
cost1.evaluator().UpdateCoefficients(new_Q=[[1., 2], [2., 4]], new_b=[1, 2.], new_c= 4)
print(f" updated cost: {cost1}")
original cost: QuadraticCost (5 + 3 * x(0) + (x(0) * (x(0) + 0.5 * x(1))) + (x(1) * (0.5 * x(0) + 2 * x(1)))) updated cost: QuadraticCost (4 + x(0) + 2 * x(1) + (x(0) * (0.5 * x(0) + x(1))) + (x(1) * (x(0) + 2 * x(1))))
For any constraint $lower \le f(x) \le upper$, we can update its bounds by
Constraint.UpdateLowerBound(new_lb) to change its lower bound to new_lb.Constraint.UpdateUpperBound(new_ub) to change its upper bound to new_ub.Constraint.set_bounds(new_lb, new_ub) up change both its lower and upper boundsprog = MathematicalProgram()
x = prog.NewContinuousVariables(2)
constraint1 = prog.AddLinearConstraint(x[0] + 3 * x[1] <= 2)
print(f"original constraint: {constraint1}")
constraint1.evaluator().UpdateLowerBound([-1])
print(f"updated constraint: {constraint1}")
constraint1.evaluator().UpdateUpperBound([3])
print(f"updated constraint: {constraint1}")
constraint1.evaluator().set_bounds(new_lb=[-5], new_ub=[10])
print(f"updated constraint: {constraint1}")
original constraint: LinearConstraint -inf <= (x(0) + 3 * x(1)) <= 2 updated constraint: LinearConstraint -1 <= (x(0) + 3 * x(1)) <= 2 updated constraint: LinearConstraint -1 <= (x(0) + 3 * x(1)) <= 3 updated constraint: LinearConstraint -5 <= (x(0) + 3 * x(1)) <= 10
For a linear constraint $lower \le Ax \le upper$, we can call LinearConstraint.UpdateCoefficients(new_A, new_lb, new_ub) to update the constraint as $new_{lb} \le new_A* x\le new_{ub}$.
For a linear equality constraint $Ax = b$, we can call LinearEqualityConstraint.UpdateCoefficients(Aeq, beq) to update the constraint to $\text{Aeq}*x=\text{beq}$.
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2)
linear_constraint = prog.AddLinearConstraint(3 * x[0] + 4 * x[1] <= 5)
linear_eq_constraint = prog.AddLinearConstraint(5 * x[0] + 2 * x[1] == 3)
print(f"original linear constraint: {linear_constraint}")
linear_constraint.evaluator().UpdateCoefficients(new_A = [[1, 3]], new_lb=[-2], new_ub=[3])
print(f"updated linear constraint: {linear_constraint}")
print(f"original linear equality constraint: {linear_eq_constraint}")
linear_eq_constraint.evaluator().UpdateCoefficients(Aeq=[[3, 4]], beq=[2])
print(f"updated linear equality constraint: {linear_eq_constraint}")
original linear constraint: LinearConstraint -inf <= (3 * x(0) + 4 * x(1)) <= 5 updated linear constraint: LinearConstraint -2 <= (x(0) + 3 * x(1)) <= 3 original linear equality constraint: LinearEqualityConstraint (5 * x(0) + 2 * x(1)) == 3 updated linear equality constraint: LinearEqualityConstraint (3 * x(0) + 4 * x(1)) == 2