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 test_snopt_solver(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] + x[1] == 1)
prog.AddBoundingBoxConstraint(0, 1, x[1])
prog.AddLinearCost(x[0])
solver = SnoptSolver()
self.assertEqual(solver.solver_id(), SnoptSolver.id())
if solver.available():
self.assertTrue(solver.enabled())
self.assertEqual(solver.solver_type(), mp.SolverType.kSnopt)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
numpy_compare.assert_float_allclose(result.GetSolution(x),
[0., 1.],
atol=1E-7)
self.assertEqual(result.get_solver_details().info, 1)
np.testing.assert_allclose(result.get_solver_details().xmul,
np.array([0., -1]))
np.testing.assert_allclose(result.get_solver_details().F,
np.array([0, 1.]))
np.testing.assert_allclose(result.get_solver_details().Fmul,
np.array([0, 1.]))
def test_two_boxes(self):
# test with 2 boxes.
masses = [1., 2]
box_sizes = [np.array([0.1, 0.1, 0.1]), np.array([0.1, 0.1, 0.2])]
plant, scene_graph, diagram, boxes, ground_geometry_id,\
boxes_geometry_id = construct_environment(
masses, box_sizes)
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
# Ignore the collision between box-box, since currently Drake doesn't
# support box-box collision with AutoDiffXd.
ignored_collision_pairs = set()
for i in range(len(boxes_geometry_id)):
ignored_collision_pairs.add(
tuple((ground_geometry_id, boxes_geometry_id[i])))
for j in range(i + 1, len(boxes_geometry_id)):
ignored_collision_pairs.add(
tuple((boxes_geometry_id[i], boxes_geometry_id[j])))
dut = StaticEquilibriumProblem(plant, plant_context,
ignored_collision_pairs)
# Add the constraint that the quaternion should have unit length.
ik.AddUnitQuaternionConstraintOnPlant(plant, dut.q_vars(),
dut.get_mutable_prog())
for i in range(len(boxes)):
box_quaternion_start = 7 * i
box_quat, box_xyz = split_se3(
dut.q_vars()[box_quaternion_start:box_quaternion_start + 7])
dut.get_mutable_prog().SetInitialGuess(
box_quat, np.array([0.5, -0.5, 0.5, 0.5]))
dut.get_mutable_prog().SetInitialGuess(
box_xyz, np.array([0, 0, 0.3 + 0.2 * i]))
dut.UpdateComplementarityTolerance(0.1)
snopt_solver = SnoptSolver()
result = snopt_solver.Solve(dut.prog())
self.assertTrue(result.is_success())
# Now progressively tighten the tolerance and solve it again.
for tol in (0.05, 0.01, 0.005, 0.001):
dut.UpdateComplementarityTolerance(tol)
dut.get_mutable_prog().SetInitialGuessForAllVariables(
result.get_x_val())
result = snopt_solver.Solve(dut.prog())
self.assertTrue(result.is_success())
def test_one_box(self):
# Test with a single box.
masses = [1.]
box_sizes = [np.array([0.1, 0.1, 0.1])]
env = construct_environment(masses, box_sizes)
diagram_context = env.diagram.CreateDefaultContext()
plant_context = env.diagram.GetMutableSubsystemContext(
env.plant, diagram_context)
# Ignore the collision between box-box, since currently Drake doesn't
# support box-box collision with AutoDiffXd.
ignored_collision_pairs = {(
env.ground_geometry_id, env.boxes_geometry_id[0])}
dut = StaticEquilibriumProblem(
env.plant, plant_context, ignored_collision_pairs)
# Add the constraint that the quaternion should have unit length.
box_quat, box_xyz = split_se3(dut.q_vars())
ik.AddUnitQuaternionConstraintOnPlant(
env.plant, dut.q_vars(), dut.get_mutable_prog())
dut.get_mutable_prog().SetInitialGuess(
box_quat, np.array([0.5, 0.5, 0.5, 0.5]))
dut.get_mutable_prog().SetInitialGuess(
box_xyz, np.array([0, 0, 0.3]))
# Now set the complementarity tolerance.
dut.UpdateComplementarityTolerance(0.002)
solver = SnoptSolver()
result = solver.Solve(dut.prog())
self.assertTrue(result.is_success())
q_sol = result.GetSolution(dut.q_vars())
box_quat_sol, box_xyz_sol = split_se3(q_sol)
self.assertAlmostEqual(np.linalg.norm(box_quat_sol), 1.)
np.testing.assert_allclose(box_xyz_sol[2], 0.05, atol=0.002)
contact_wrenches = dut.GetContactWrenchSolution(result)
self.assertEqual(len(contact_wrenches), 8)
prog.SetSolverOption(SnoptSolver().solver_id(), "Iterations Limit", 1e5)
prog.SetSolverOption(SnoptSolver().solver_id(), "Major Feasibility Tolerance",
1e-6)
prog.SetSolverOption(SnoptSolver().solver_id(), "Major Optimality Tolerance",
1e-6)
prog.SetSolverOption(SnoptSolver().solver_id(), "Scale Option", 2)
solver = SnoptSolver()
# trajopt.enable_cost_display(display='figure')
# Check the problem for bugs in the constraints
if not utils.CheckProgram(prog):
quit()
# Solve the problem
print("Solving trajectory optimization")
start = timeit.default_timer()
result = solver.Solve(prog)
stop = timeit.default_timer()
print(f"Elapsed time: {stop-start}")
# Print the details of the solution
print(f"Optimization successful? {result.is_success()}")
print(f"Solved with {result.get_solver_id().name()}")
print(f"Optimal cost = {result.get_optimal_cost()}")
# Get the exit code from SNOPT
print(
f"SNOPT Exit Status {result.get_solver_details().info}: {utils.SNOPT_DECODER[result.get_solver_details().info]}"
)
# Unpack and plot the trajectories
x = trajopt.reconstruct_state_trajectory(result)
u = trajopt.reconstruct_input_trajectory(result)
l = trajopt.reconstruct_reaction_force_trajectory(result)
