def test_infeasible_instance(basic_init_fixture, solver_name):
"""If the given parameters are infeasible, the solverwrapper should
terminate gracefully and return a numpy vector [nan, nan].
"""
constraints, path, path_discretization, vlim, alim = basic_init_fixture
if solver_name == "cvxpy":
solver = cvxpyWrapper(constraints, path, path_discretization)
elif solver_name == 'qpOASES':
solver = qpOASESSolverWrapper(constraints, path, path_discretization)
elif solver_name == 'hotqpOASES':
solver = hotqpOASESSolverWrapper(constraints, path,
path_discretization)
elif solver_name == 'ecos':
solver = ecosWrapper(constraints, path, path_discretization)
elif solver_name == 'seidel':
solver = seidelWrapper(constraints, path, path_discretization)
g = np.r_[0, 1].astype(float)
solver.setup_solver()
result = solver.solve_stagewise_optim(0, None, g, 1.1, 1.0, np.nan, np.nan)
assert np.all(np.isnan(result))
result = solver.solve_stagewise_optim(0, None, g, 1.1, 1.0, 0, -0.5)
assert np.all(np.isnan(result))
result = solver.solve_stagewise_optim(0, None, g, np.nan, np.nan, 0, -0.5)
assert np.all(np.isnan(result))
solver.close_solver()
def test_basic_init(basic_init_fixture, solver_name, i, H, g, x_ineq):
""" A basic test case for wrappers.
Notice that the input fixture `basic_init_fixture` is known to have two constraints,
one velocity and one acceleration. Hence, in this test, I directly formulate
an optimization with cvxpy to test the result.
Parameters
----------
basic_init_fixture: a fixture with only two constraints, one velocity and
one acceleration constraint.
"""
constraints, path, path_discretization, vlim, alim = basic_init_fixture
if solver_name == "cvxpy":
solver = cvxpyWrapper(constraints, path, path_discretization)
elif solver_name == 'qpOASES':
solver = qpOASESSolverWrapper(constraints, path, path_discretization)
elif solver_name == 'hotqpOASES':
solver = hotqpOASESSolverWrapper(constraints, path,
path_discretization)
elif solver_name == 'ecos' and H is None:
solver = ecosWrapper(constraints, path, path_discretization)
elif solver_name == 'seidel' and H is None:
solver = seidelWrapper(constraints, path, path_discretization)
else:
return True # Skip all other tests
xmin, xmax = x_ineq
xnext_min = 0
xnext_max = 1
# Results from solverwrapper to test
solver.setup_solver()
result_ = solver.solve_stagewise_optim(i - 2, H, g, xmin, xmax, xnext_min,
xnext_max)
result_ = solver.solve_stagewise_optim(i - 1, H, g, xmin, xmax, xnext_min,
xnext_max)
result = solver.solve_stagewise_optim(i, H, g, xmin, xmax, xnext_min,
xnext_max)
solver.close_solver()
# Results from cvxpy, used as the actual, desired values
ux = cvxpy.Variable(2)
u = ux[0]
x = ux[1]
_, _, _, _, _, _, xbound = solver.params[0] # vel constraint
a, b, c, F, h, ubound, _ = solver.params[1] # accel constraint
a2, b2, c2, F2, h2, _, _ = solver.params[2] # random constraint
Di = path_discretization[i + 1] - path_discretization[i]
v = a[i] * u + b[i] * x + c[i]
v2 = a2[i] * u + b2[i] * x + c2[i]
cvxpy_constraints = [
u <= ubound[i, 1],
u >= ubound[i, 0],
x <= xbound[i, 1],
x >= xbound[i, 0],
F * v <= h,
F2[i] * v2 <= h2[i],
x + u * 2 * Di <= xnext_max,
x + u * 2 * Di >= xnext_min,
]
if not np.isnan(xmin):
cvxpy_constraints.append(x <= xmax)
cvxpy_constraints.append(x >= xmin)
if H is not None:
objective = cvxpy.Minimize(0.5 * cvxpy.quad_form(ux, H) + g * ux)
else:
objective = cvxpy.Minimize(g * ux)
problem = cvxpy.Problem(objective, cvxpy_constraints)
if FOUND_MOSEK:
problem.solve(solver="MOSEK", verbose=True)
else:
problem.solve(solver="ECOS", verbose=True)
if problem.status == "optimal":
actual = np.array(ux.value).flatten()
result = np.array(result).flatten()
npt.assert_allclose(result, actual, atol=5e-3,
rtol=1e-5) # Very bad accuracy? why?
else:
assert np.all(np.isnan(result))