def find_feasible_latents(self, observed):
# Build an optimization to estimate the hidden variables
try:
prog = MathematicalProgram()
# Add in all the appropriate variables with their bounds
all_vars = self.prices[0].GetVariables()
for price in self.prices[1:]:
all_vars += price.GetVariables()
mp_vars = prog.NewContinuousVariables(len(all_vars))
subs_dict = {}
for v, mv in zip(all_vars, mp_vars):
subs_dict[v] = mv
lb = []
ub = []
prog.AddBoundingBoxConstraint(0., 1., mp_vars)
prices_mp = [
self.prices[k].Substitute(subs_dict) for k in range(12)
]
# Add the observation constraint
for k, val in enumerate(observed[1:]):
if val != 0:
prog.AddConstraint(prices_mp[k] >= val - 2.)
prog.AddConstraint(prices_mp[k] <= val + 2)
# Find lower bounds
prog.AddCost(sum(prices_mp))
solver = SnoptSolver()
result = solver.Solve(prog)
if result.is_success():
lb = [result.GetSolution(x).Evaluate() for x in prices_mp]
lb_vars = result.GetSolution(mp_vars)
# Find upper bound too
prog.AddCost(-2. * sum(prices_mp))
result = solver.Solve(prog)
if result.is_success():
ub_vars = result.GetSolution(mp_vars)
ub = [result.GetSolution(x).Evaluate() for x in prices_mp]
self.price_ub = ub
self.price_lb = lb
subs_dict = {}
for k, v in enumerate(all_vars):
if lb_vars[k] == ub_vars[k]:
subs_dict[v] = lb_vars[k]
else:
new_var = sym.Variable(
"feasible_%d" % k,
sym.Variable.Type.RANDOM_UNIFORM)
subs_dict[v] = new_var * (ub_vars[k] -
lb_vars[k]) + lb_vars[k]
self.prices = [
self.prices[k].Substitute(subs_dict) for k in range(12)
]
return
except RuntimeError as e:
print("Runtime error: ", e)
self.rollout_probability = 0.
def minimize_height(diagram_f, plant_f, d_context_f, frame_list):
"""Fragile and slow :("""
context_f = plant_f.GetMyContextFromRoot(d_context_f)
diagram_ad = diagram_f.ToAutoDiffXd()
plant_ad = diagram_ad.GetSubsystemByName(plant_f.get_name())
d_context_ad = diagram_ad.CreateDefaultContext()
d_context_ad.SetTimeStateAndParametersFrom(d_context_f)
context_ad = plant_ad.GetMyContextFromRoot(d_context_ad)
def prepare_plant_and_context(z):
if z.dtype == float:
plant, context = plant_f, context_f
else:
plant, context = plant_ad, context_ad
set_frame_heights(plant, context, frame_list, z)
return plant, context
prog = MathematicalProgram()
num_z = len(frame_list)
z_vars = prog.NewContinuousVariables(num_z, "z")
q0 = plant_f.GetPositions(context_f)
z0 = get_frame_heights(plant_f, context_f, frame_list)
cost = prog.AddCost(np.sum(z_vars))
prog.AddBoundingBoxConstraint([0.01] * num_z, [5.] * num_z, z_vars)
# # N.B. Cannot use AutoDiffXd due to cylinders.
distance = MinimumDistanceConstraint(plant=plant_f,
plant_context=context_f,
minimum_distance=0.05)
def distance_with_z(z):
plant, context = prepare_plant_and_context(z)
q = plant.GetPositions(context)
return distance.Eval(q)
prog.AddConstraint(distance_with_z,
lb=distance.lower_bound(),
ub=distance.upper_bound(),
vars=z_vars)
result = Solve(prog, initial_guess=z0)
assert result.is_success()
z = result.GetSolution(z_vars)
set_frame_heights(plant_f, context_f, frame_list, z)
q = plant_f.GetPositions(context_f)
return q
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
y = prog.NewContinuousVariables(3, "y")
bounding_box = prog.AddLinearConstraint(x[1] <= 2)
linear_eq = prog.AddLinearConstraint(x[1] + 2 * y[2] == 1)
linear_ineq = prog.AddLinearConstraint(x[1] + 4 * y[1] >= 2)
# New program, all the way to solving...
prog = MathematicalProgram()
# Declare x as decision variables.
x = prog.NewContinuousVariables(4)
# Add linear costs. To show that calling AddLinearCosts results in the sum of each individual
# cost, we add two costs -3x[0] - x[1] and -5x[2]-x[3]+2
prog.AddLinearCost(-3*x[0] -x[1])
prog.AddLinearCost(-5*x[2] - x[3] + 2)
# Add linear equality constraint 3x[0] + x[1] + 2x[2] == 30
prog.AddLinearConstraint(3*x[0] + x[1] + 2*x[2] == 30)
# Add Linear inequality constraints
prog.AddLinearConstraint(2*x[0] + x[1] + 3*x[2] + x[3] >= 15)
prog.AddLinearConstraint(2*x[1] + 3*x[3] <= 25)
# Add linear inequality constraint -100 <= x[0] + 2x[2] <= 40
prog.AddLinearConstraint(A=[[1., 2.]], lb=[-100], ub=[40], vars=[x[0], x[2]])
prog.AddBoundingBoxConstraint(0, np.inf, x)
prog.AddLinearConstraint(x[1] <= 10)
# Now solve the program.
result = Solve(prog)
print(f"Is solved successfully: {result.is_success()}")
print(f"x optimal value: {result.GetSolution(x)}")
print(f"optimal cost: {result.get_optimal_cost()}")
def plot_sublevelset_expression(ax, e, vertices=51, **kwargs):
"""
Plots the 2D sub-level set e(x) <= 1, which must contain the origin.
Args:
ax: the matplotlib axis to receive the plot
e: a symbolic expression in two variables
vertices: number of sample points along the boundary
kwargs: are passed to the matplotlib fill method
Returns:
the return values from matplotlib's fill command.
"""
x = list(e.GetVariables())
assert len(x) == 2, "e must be an expression in two variables"
# Handle the special case where e is a degree 2 polynomial.
if e.is_polynomial():
p = Polynomial(e)
if p.TotalDegree() == 2:
env = {a: 0 for a in x}
c = e.Evaluate(env)
e1 = e.Jacobian(x)
b = Evaluate(e1, env)
e2 = Jacobian(e1, x)
A = 0.5 * Evaluate(e2, env)
return plot_sublevelset_quadratic(ax, A, b, c, vertices, **kwargs)
# Find the level-set in polar coordinates, by sampling theta and
# root-finding (on the scalar expression) to find a rplus and rminus.
Xplus = np.empty((2, vertices))
Xminus = np.empty((2, vertices))
i = 0
for theta in np.linspace(0, np.pi, vertices):
prog = MathematicalProgram()
r = prog.NewContinuousVariables(1, "r")[0]
env = {x[0]: r * np.cos(theta), x[1]: r * np.sin(theta)}
scalar = e.Substitute(env)
b = prog.AddBoundingBoxConstraint(0, np.inf, r)
prog.AddConstraint(scalar == 1)
prog.AddQuadraticCost([1], [0], [r])
prog.SetInitialGuess(r, 0.1) # or anything non-zero.
result = Solve(prog)
assert result.is_success(), "Failed to find the level set"
rplus = result.GetSolution(r)
Xplus[0, i] = rplus * np.cos(theta)
Xplus[1, i] = rplus * np.sin(theta)
b.evaluator().UpdateLowerBound([-np.inf])
b.evaluator().UpdateUpperBound([0])
prog.SetInitialGuess(r, -0.1) # or anything non-zero.
result = Solve(prog)
assert result.is_success(), "Failed to find the level set"
rminus = result.GetSolution(r)
Xminus[0, i] = rminus * np.cos(theta)
Xminus[1, i] = rminus * np.sin(theta)
i = i + 1
return ax.fill(np.hstack((Xplus[0, :], Xminus[0, :])),
np.hstack((Xplus[1, :], Xminus[1, :])), **kwargs)