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 findMaxForce(theta1, theta2, theta3):
theta12 = theta2 + theta1
theta123 = theta3 + theta12
s1 = sin(theta1)
s2 = sin(theta2)
s3 = sin(theta3)
s12 = sin(theta12)
s123 = sin(theta123)
c1 = cos(theta1)
c2 = cos(theta2)
c3 = cos(theta3)
c12 = cos(theta12)
c123 = cos(theta123)
J = np.array([[
-l_1 * s1 - l_2 * s12 - l_3 * s123, -l_2 * s12 - l_3 * s123,
-l_3 * s123
], [l_1 * c1 + l_2 * c12 + l_3 * c123, l_2 * c12 + l_3 * c123,
l_3 * c123]])
tau1 = (
-l_1 * c1 * m_2 * g # torque due to l_1 - l_2 link motor
+ (-l_1 * c1 + l_2 / 2.0 * c12) * m_m *
g # torque due to battery link motor
+ (-l_1 * c1 + l_2 / 2.0 * c12 + l_b * cos(theta12 + np.pi / 2.0)) *
m_b * g # torque due to battery
+ (-l_1 * c1 + l_2) * m_3 * g # torque due to l_2 - l_3 link motor
+ (-l_1 * c1 + l_2 + l_3 * c3) * m_w * g # torque due to front wheel
)
# print("tau1 = " + str(tau1))
mp = MathematicalProgram()
tau23 = mp.NewContinuousVariables(2, "tau23")
tau123 = np.append(tau1, tau23)
mp.AddCost(J.dot(tau123)[0])
mp.AddConstraint(tau23[0]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2)
mp.AddConstraint(tau23[1]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2)
mp.AddLinearConstraint(J.dot(tau123)[1] >= -HUB_MOTOR_FORCE / 2.0)
result = Solve(mp)
is_success = result.is_success()
torques = result.GetSolution(tau23)
full_tau = np.append(tau1, torques)
output_force = J.dot(full_tau)
# print("is_success = " + str(is_success))
# print("tau = " + str(full_tau))
# print("F = " + str(J.dot(full_tau)))
return is_success, output_force, torques
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
# "Unable to cast Python instance to c++ type"
link_7_distance_to_target_vector = lambda q: [link_7_distance_to_target(q)]
# New program: Using a custom evaluator
prog = MathematicalProgram()
q = prog.NewContinuousVariables(plant_f.num_positions())
# Define nominal configuration
q0 = np.zeros(plant_f.num_positions())
# Add basic cost. (This will be parsed into a quadratic cost)
prog.AddCost((q - q0).dot(q - q0))
# Add constraint based on custom evaluator.
prog.AddConstraint(link_7_distance_to_target_vector,
lb=[0.1],
ub=[0.2],
vars=q)
result = Solve(prog, initial_guess=q0)
print(f"Success? {result.is_success()}")
print(result.get_solution_result())
q_sol = result.GetSolution(q)
print(q_sol)
prog = MathematicalProgram()
q = prog.NewContinuousVariables(plant_f.num_positions())
# Define nominal configuration.
q0 = np.zeros(plant_f.num_positions())
this example also demonstrates how to add a callback to the program, and use it to visualize the progression of the solver.
Adapted from the MathematicalProgram tutorial on the Drake website: https://drake.mit.edu
"""
from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve
import numpy as np
import matplotlib.pyplot as plt
# Create empty MathematicalProgram
prog = MathematicalProgram()
# Add 2 continuous decision variables
# x is a numpy array - we can use an optional second argument to name the variables
x = prog.NewContinuousVariables(2)
#print(x)
prog.AddConstraint(x[0] + x[1] == 1)
prog.AddConstraint(x[0] <= x[1])
prog.AddCost(x[0]**2 + x[1]**2)
# Make and add a visualization callback
fig = plt.figure()
curve_x = np.linspace(1, 10, 100)
ax = plt.gca()
ax.plot(curve_x, 9. / curve_x)
ax.plot(-curve_x, -9. / curve_x)
ax.plot(0, 0, 'o')
x_init = [4., 5.]
point_x, = ax.plot(x_init[0], x_init[1], 'x')
ax.axis('equal')
def compute_input(self, x, xd, initial_guess=None):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda1')
z = prog.NewBinaryVariables(rows=self.T, cols=self.nf, name='z')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
prog.AddConstraint(z[0, 0] == 0)
prog.AddConstraint(z[0, 1] == 0)
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints with big M = self.contact
prog.AddConstraint(ge(contact[t], 0))
prog.AddConstraint(contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) >= 0)
prog.AddConstraint(contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) >= 0)
# Mixed Integer Constraints
M = self.contact_max
prog.AddConstraint(contact[t, 0] <= M)
prog.AddConstraint(contact[t, 1] <= M)
prog.AddConstraint(contact[t, 0] <= M * z[t, 0])
prog.AddConstraint(contact[t, 1] <= M * z[t, 1])
prog.AddConstraint(
contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) <= M *
(1 - z[t, 0]))
prog.AddConstraint(
contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) <= M *
(1 - z[t, 1]))
prog.AddConstraint(z[t, 0] + z[t, 1] == 1)
# Add Input Constraints. Contact Constraints already enforced in big-M
# prog.AddConstraint(le(u[t], self.input_max))
# prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver_id = IpoptSolver().solver_id()
elif (self.solver == "snopt"):
solver_id = SnoptSolver().solver_id()
elif (self.solver == "osqp"):
solver_id = OsqpSolver().solver_id()
elif (self.solver == "mosek"):
solver_id = MosekSolver().solver_id()
elif (self.solver == "gurobi"):
solver_id = GurobiSolver().solver_id()
solver = MixedIntegerBranchAndBound(prog, solver_id)
#result = solver.Solve(prog, initial_guess)
result = solver.Solve()
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem')
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt
prog = MathematicalProgram() x = prog.NewContinuousVariables(2) print(x) print(1 + 2 * x[0] + 3 * x[1] + 4 * x[1]) y = prog.NewContinuousVariables(2, "dog") print(y) print(y[0] + y[0] + y[1] * y[1] * y[1]) var_matrix = prog.NewContinuousVariables(3, 2, "A") print(var_matrix) # Add the constraint x(0) * x(1) = 1 to prog prog.AddConstraint(x[0] * x[1] == 1) prog.AddConstraint(x[0] >= 0) prog.AddConstraint(x[0] - x[1] <= 0) prog.AddCost(x[0]**2 + 3) prog.AddCost(x[0] + x[1]) # New optimization program, all the way through to solving prog = MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddConstraint(x[0] + x[1] == 1) prog.AddConstraint(x[0] <= x[1]) prog.AddCost(x[0]**2 + x[1]**2) # Now solve the optimization problem result = Solve(prog)
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)
tau234_over_time = np.zeros(shape=(NUM_TIME_STEPS, TORQUE_SIZE),
dtype=pydrake.symbolic.Variable)
initial_state = mp.NewContinuousVariables(8, "state_0")
initial_theta1 = initial_state[0]
initial_theta2 = initial_state[1]
initial_theta3 = initial_state[2]
initial_theta4 = initial_state[3]
# Constrain initial velocity to be 0
# for j in range(4, 8):
# mp.AddConstraint(initial_state[j] <= 0.0).evaluator().set_description("Constrain initial_state[%d] <= 0.0" % j)
# mp.AddConstraint(initial_state[j] >= 0.0).evaluator().set_description("Constrain initial_state[%d] >= 0.0" % j)
# constrain_theta123(mp, initial_theta1, initial_theta2, initial_theta3)
mp.AddConstraint(initial_theta1 <= -np.pi / 3.0)
mp.AddConstraint(initial_theta1 >= -np.pi / 3.0)
mp.AddConstraint(initial_theta2 <= np.pi / 3.0)
mp.AddConstraint(initial_theta2 >= np.pi / 3.0)
# mp.AddConstraint(initial_theta4 >= 0.0)
# mp.AddConstraint(initial_theta4 <= 0.0)
# Constrain initial front wheel position y position to be 0.0
initial_wheel_position = eom.findFrontWheelPosition(
initial_state[0], initial_state[1], initial_state[2])
mp.AddConstraint(initial_wheel_position[1] <= 0.0).evaluator(
).set_description("Constrain initial_wheel_position[1] <= 0.0")
mp.AddConstraint(initial_wheel_position[1] >= 0.0).evaluator(
).set_description("Constrain initial_wheel_position[1] >= 0.0")
state_over_time[0] = initial_state
def compute_input(self, x, xd, initial_guess=None, tol=0.0):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda')
#
alpha = prog.NewContinuousVariables(rows=self.T, cols=2, name='alpha')
beta = prog.NewContinuousVariables(rows=self.T, cols=2, name='beta')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints
prog.AddConstraint(ge(alpha[t], 0))
prog.AddConstraint(ge(beta[t], 0))
prog.AddConstraint(alpha[t, 0] == contact[t, 0])
prog.AddConstraint(alpha[t, 1] == contact[t, 1])
prog.AddConstraint(
beta[t, 0] == (contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d'])))
prog.AddConstraint(
beta[t, 1] == (contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d'])))
# Complementarity constraints. Start with relaxed version and start constraining.
prog.AddConstraint(alpha[t, 0] * beta[t, 0] <= tol)
prog.AddConstraint(alpha[t, 1] * beta[t, 1] <= tol)
# Add Input Constraints and Contact Constraints
prog.AddConstraint(le(contact[t], self.contact_max))
prog.AddConstraint(ge(contact[t], -self.contact_max))
prog.AddConstraint(le(u[t], self.input_max))
prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver = IpoptSolver()
elif (self.solver == "snopt"):
solver = SnoptSolver()
result = solver.Solve(prog, initial_guess)
if (self.solver == "ipopt"):
print("Ipopt Solver Status: ",
result.get_solver_details().status, ", meaning ",
result.get_solver_details().ConvertStatusToString())
elif (self.solver == "snopt"):
val = result.get_solver_details().info
status = self.snopt_status(val)
print("Snopt Solver Status: ",
result.get_solver_details().info, ", meaning ", status)
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt