def test_ipopt_solver_deprecated(self):
# This serves as the (only) unit test coverage for the deprecated
# SolverBase method that writes the solution back into the program.
prog, x, x_expected = self._make_prog()
solver = IpoptSolver()
with catch_drake_warnings(expected_count=2):
result = solver.Solve(prog)
solution = prog.GetSolution(x)
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(solution, x_expected))
def test_ipopt_solver(self):
prog, x, x_expected = self._make_prog()
solver = IpoptSolver()
self.assertTrue(solver.available())
self.assertEqual(solver.solver_id().name(), "IPOPT")
self.assertEqual(solver.SolverName(), "IPOPT")
self.assertEqual(solver.solver_type(), mp.SolverType.kIpopt)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
def test_ipopt_solver_deprecated(self):
# This serves as the (only) unit test coverage for the deprecated
# SolverBase method that writes the solution back into the program.
prog, x, x_expected = self._make_prog()
solver = IpoptSolver()
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
result = solver.Solve(prog)
solution = prog.GetSolution(x)
self.assertEqual(len(w), 2)
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(solution, x_expected))
def test_ipopt_solver(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] >= 1)
prog.AddLinearConstraint(x[1] >= 1)
prog.AddQuadraticCost(np.eye(2), np.zeros(2), x)
solver = IpoptSolver()
self.assertTrue(solver.available())
self.assertEqual(solver.SolverName(), "IPOPT")
self.assertEqual(solver.solver_type(), mp.SolverType.kIpopt)
result = solver.Solve(prog)
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))
def test_ipopt_solver(self):
prog, x, x_expected = self._make_prog()
solver = IpoptSolver()
self.assertTrue(solver.available())
self.assertEqual(solver.solver_id().name(), "IPOPT")
self.assertEqual(solver.SolverName(), "IPOPT")
self.assertEqual(solver.solver_type(), mp.SolverType.kIpopt)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
self.assertEqual(result.get_solver_details().status, 0)
self.assertEqual(result.get_solver_details().ConvertStatusToString(),
"Success")
np.testing.assert_allclose(result.get_solver_details().z_L,
np.array([1., 1.]))
np.testing.assert_allclose(result.get_solver_details().z_U,
np.array([0., 0.]))
np.testing.assert_allclose(result.get_solver_details().g, np.array([]))
np.testing.assert_allclose(result.get_solver_details().lambda_val,
np.array([]))
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.001)
ipopt_solver = IpoptSolver()
result = ipopt_solver.Solve(dut.prog())
self.assertTrue(result.is_success())
# prog.AddConstraint(eq(x[2,n+1], x[2,n] + dt*dx[2,n]))
prog.AddBoundingBoxConstraint([-f*m*g]*2, [f*m*g]*2, u[:,n])
prog.AddBoundingBoxConstraint(-5, 5, x[0,n])
prog.AddBoundingBoxConstraint(-5, 5, x[1,n])
prog.AddBoundingBoxConstraint(-np.pi, np.pi, x[2,n])
xf = [0., 0., 0.]
prog.AddBoundingBoxConstraint(xf, xf, x[:, N-1])
uf = [m*g/2, m*g/2]
prog.AddBoundingBoxConstraint(uf, uf, u[:, N-1])
x1 = [0]*3
prog.AddBoundingBoxConstraint(x1, x1, dx[:, N-1])
solver = IpoptSolver()
result = solver.Solve(prog)
# result = Solve(prog)
x_sol = result.GetSolution(x)
dx_sol = result.GetSolution(dx)
u_sol = result.GetSolution(u)
assert(result.is_success()), "Optimization failed"
time = np.linspace(0, T, N)
# print(len(time))
plt.figure()
plt.subplot(311)
plt.plot(time, x_sol[0,:], 'r', label='x')
plt.plot(time, x_sol[1,:], 'g', label='y')
plt.plot(time, x_sol[2,:], 'b', label='theta')
plt.subplot(312)
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