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)
SolverId, SolverInterface) from functools import partial import unittest import warnings import numpy as np import pydrake from pydrake.autodiffutils import AutoDiffXd from pydrake.common.test_utilities import numpy_compare from pydrake.forwarddiff import jacobian from pydrake.math import ge import pydrake.symbolic as sym SNOPT_NO_GUROBI = SnoptSolver().available() and not GurobiSolver().available() class TestQP: def __init__(self): # Create a simple QP that uses all deduced linear constraint types, # along with a quadratic and linear cost. # The solution should be [1, 1]. prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, "x") self.prog = prog self.x = x self.constraints = [ # Bounding box prog.AddLinearConstraint(x[0] >= 1), # Bounding box
trajopt.add_quadratic_running_cost(Q, xf, [trajopt.x], name="StateCost") # Add a final cost on the total time # cost = lambda h: np.sum(h) # trajopt.add_final_cost(cost, vars=[trajopt.h], name="TotalTime") # Set the initial trajectory guess u_init = np.zeros(trajopt.u.shape) x_init = np.zeros(trajopt.x.shape) for n in range(0, x_init.shape[0]): x_init[n, :] = np.linspace(start=x0[n], stop=xf[n], num=101) l_init = np.zeros(trajopt.l.shape) trajopt.set_initial_guess(xtraj=x_init, utraj=u_init, ltraj=l_init) # Get the final program, with all costs and constraints prog = trajopt.get_program() # Set the SNOPT solver options 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)
trajopt.add_equal_time_constraints() # Add control and state costs # Add a running cost weights on the controls R = 100 * np.ones((1, 1)) b = np.zeros((1, )) # Add running cost weight on state Q = 1 * np.diag([1, 1, 1, 1]) trajopt.add_quadratic_running_cost(R, b, [trajopt.u], name="ControlCost") trajopt.add_quadratic_running_cost(Q, xf, [trajopt.x], name="StateCost") u_init = u_init.reshape(trajopt.u.shape) # Set the initial trajectory guess, might switch out later for a warm start trajectory trajopt.set_initial_guess(xtraj=x_init, utraj=u_init, ltraj=l_init) prog = trajopt.get_program() # Set the SNOPT solver options prog.SetSolverOption(SnoptSolver().solver_id(), "Iterations Limit", 1e7) if i == 1: 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", 1) else: 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() # Solve the problem
def do_main(): rospy.init_node('run_dishrack_interaction', anonymous=False) #np.random.seed(42) for outer_iter in range(200): try: builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.0025)) # Add ground world_body = mbp.world_body() ground_shape = Box(2., 2., 2.) ground_body = mbp.AddRigidBody( "ground", SpatialInertia(mass=10.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(), RigidTransform(p=[0, 0, -1])) mbp.RegisterVisualGeometry(ground_body, RigidTransform.Identity(), ground_shape, "ground_vis", np.array([0.5, 0.5, 0.5, 1.])) mbp.RegisterCollisionGeometry(ground_body, RigidTransform.Identity(), ground_shape, "ground_col", CoulombFriction(0.9, 0.8)) parser = Parser(mbp, scene_graph) dish_bin_model = "/home/gizatt/projects/scene_generation/models/dish_models/bus_tub_01_decomp/bus_tub_01_decomp.urdf" cupboard_model = "/home/gizatt/projects/scene_generation/models/dish_models/shelf_two_levels.sdf" candidate_model_files = { #"mug": "/home/gizatt/drake/manipulation/models/mug/mug.urdf", "mug_1": "/home/gizatt/projects/scene_generation/models/dish_models/mug_1_decomp/mug_1_decomp.urdf", #"plate_11in": "/home/gizatt/drake/manipulation/models/dish_models/plate_11in_decomp/plate_11in_decomp.urdf", #"/home/gizatt/drake/manipulation/models/mug_big/mug_big.urdf", #"/home/gizatt/drake/manipulation/models/dish_models/bowl_6p25in_decomp/bowl_6p25in_decomp.urdf", #"/home/gizatt/drake/manipulation/models/dish_models/plate_8p5in_decomp/plate_8p5in_decomp.urdf", } # Decide how many of each object to add max_num_objs = 6 num_objs = [ np.random.randint(0, max_num_objs) for k in range(len(candidate_model_files.keys())) ] # Actually produce their initial poses + add them to the sim poses = [] # [quat, pos] all_object_instances = [] all_manipulable_body_ids = [] total_num_objs = sum(num_objs) object_ordering = list(range(total_num_objs)) k = 0 random.shuffle(object_ordering) print("ordering: ", object_ordering) for class_k, class_entry in enumerate( candidate_model_files.items()): for model_instance_k in range(num_objs[class_k]): class_name, class_path = class_entry model_name = "%s_%d" % (class_name, model_instance_k) all_object_instances.append([class_name, model_name]) model_id = parser.AddModelFromFile(class_path, model_name=model_name) all_manipulable_body_ids += mbp.GetBodyIndices(model_id) # Put them in a randomly ordered line, for placing y_offset = (object_ordering[k] / float(total_num_objs) - 0.5) # RAnge -0.5 to 0.5 poses.append([ RollPitchYaw(np.random.uniform( 0., 2. * np.pi, size=3)).ToQuaternion().wxyz(), [-0.25, y_offset, 0.1] ]) k += 1 #$poses.append([ # RollPitchYaw(np.random.uniform(0., 2.*np.pi, size=3)).ToQuaternion().wxyz(), # [np.random.uniform(-0.2, 0.2), np.random.uniform(-0.1, 0.1), np.random.uniform(0.1, 0.3)]]) # Build a desk parser.AddModelFromFile(cupboard_model) mbp.WeldFrames(world_body.body_frame(), mbp.GetBodyByName("shelf_origin_body").body_frame(), RigidTransform(p=[0.0, 0, 0.0])) #parser.AddModelFromFile(dish_bin_model) #mbp.WeldFrames(world_body.body_frame(), mbp.GetBodyByName("bus_tub_01_decomp_body_link").body_frame(), # RigidTransform(p=[0.0, 0., 0.], rpy=RollPitchYaw(np.pi/2., 0., 0.))) mbp.AddForceElement(UniformGravityFieldElement()) mbp.Finalize() hydra_sg_spy = builder.AddSystem( HydraInteractionLeafSystem( mbp, scene_graph, all_manipulable_body_ids=all_manipulable_body_ids)) #builder.Connect(scene_graph.get_query_output_port(), # hydra_sg_spy.get_input_port(0)) builder.Connect(scene_graph.get_pose_bundle_output_port(), hydra_sg_spy.get_input_port(0)) builder.Connect(mbp.get_state_output_port(), hydra_sg_spy.get_input_port(1)) builder.Connect(hydra_sg_spy.get_output_port(0), mbp.get_applied_spatial_force_input_port()) visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url="tcp://127.0.0.1:6000", draw_period=0.01)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext( mbp, diagram_context) sg_context = diagram.GetMutableSubsystemContext( scene_graph, diagram_context) q0 = mbp.GetPositions(mbp_context).copy() for k in range(len(poses)): offset = k * 7 q0[(offset):(offset + 4)] = poses[k][0] q0[(offset + 4):(offset + 7)] = poses[k][1] mbp.SetPositions(mbp_context, q0) simulator = Simulator(diagram, diagram_context) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) simulator.Initialize() ik = InverseKinematics(mbp, mbp_context) q_dec = ik.q() prog = ik.prog() def squaredNorm(x): return np.array([x[0]**2 + x[1]**2 + x[2]**2 + x[3]**2]) for k in range(len(poses)): # Quaternion norm prog.AddConstraint(squaredNorm, [1], [1], q_dec[(k * 7):(k * 7 + 4)]) # Trivial quaternion bounds prog.AddBoundingBoxConstraint(-np.ones(4), np.ones(4), q_dec[(k * 7):(k * 7 + 4)]) # Conservative bounds on on XYZ prog.AddBoundingBoxConstraint(np.array([-2., -2., -2.]), np.array([2., 2., 2.]), q_dec[(k * 7 + 4):(k * 7 + 7)]) def vis_callback(x): vis_diagram_context = diagram.CreateDefaultContext() mbp.SetPositions( diagram.GetMutableSubsystemContext(mbp, vis_diagram_context), x) pose_bundle = scene_graph.get_pose_bundle_output_port().Eval( diagram.GetMutableSubsystemContext(scene_graph, vis_diagram_context)) context = visualizer.CreateDefaultContext() context.FixInputPort(0, AbstractValue.Make(pose_bundle)) visualizer.Publish(context) prog.AddVisualizationCallback(vis_callback, q_dec) prog.AddQuadraticErrorCost(np.eye(q0.shape[0]) * 1.0, q0, q_dec) ik.AddMinimumDistanceConstraint(0.001, threshold_distance=1.0) prog.SetInitialGuess(q_dec, q0) print("Solving") # print "Initial guess: ", q0 start_time = time.time() solver = SnoptSolver() #solver = NloptSolver() sid = solver.solver_type() # SNOPT prog.SetSolverOption(sid, "Print file", "test.snopt") prog.SetSolverOption(sid, "Major feasibility tolerance", 1e-3) prog.SetSolverOption(sid, "Major optimality tolerance", 1e-2) prog.SetSolverOption(sid, "Minor feasibility tolerance", 1e-3) prog.SetSolverOption(sid, "Scale option", 0) #prog.SetSolverOption(sid, "Elastic weight", 1e1) #prog.SetSolverOption(sid, "Elastic mode", "Yes") # NLOPT #prog.SetSolverOption(sid, "initial_step", 0.1) #prog.SetSolverOption(sid, "xtol_rel", 1E-2) #prog.SetSolverOption(sid, "xtol_abs", 1E-2) #prog.SetSolverOption(sid, "Major step limit", 2) print("Solver opts: ", prog.GetSolverOptions(solver.solver_type())) result = mp.Solve(prog) print("Solve info: ", result) print("Solved in %f seconds" % (time.time() - start_time)) #print(IpoptSolver().Solve(prog)) print(result.get_solver_id().name()) q0_proj = result.GetSolution(q_dec) # print "Final: ", q0_proj mbp.SetPositions(mbp_context, q0_proj) simulator.StepTo(1000) raise StopIteration() except StopIteration: print(colored("Stopped, saving and restarting", 'yellow')) qf = mbp.GetPositions(mbp_context) # Decide whether to accept: all objs within bounds save = True for k in range(len(all_object_instances)): offset = k * 7 q = qf[offset:(offset + 7)] if q[4] <= -0.25 or q[4] >= 0.25 or q[5] <= -0.2 or q[ 5] >= 0.2 or q[6] <= -0.1: save = False break if save: print(colored("Saving", "green")) save_config(all_object_instances, qf, "mug_rack_environments_human.yaml") else: print( colored("Not saving due to bounds violation: " + str(q), "yellow")) except Exception as e: print(colored("Suffered other exception " + str(e), "red")) sys.exit(-1) except: print( colored("Suffered totally unknown exception! Probably sim.", "red"))
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
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