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)