def test_compare_accel_robust_accel(vel_accel_robustaccel, path, solver_name,
i, H, g, x_ineq):
"""Case 4: If robust accel has very small perturbation ellipsoid, it
should be equivalent to acceleration constraint.
"""
vel_c, acc_c, _ = vel_accel_robustaccel
robust_acc_c = toppra.constraint.RobustLinearConstraint(
acc_c, [0, 0, 0],
discretization_scheme=acc_c.get_discretization_type())
path_dist = np.linspace(0, path.get_duration(), 10)
if solver_name == "cvxpy":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
solver = cvxpyWrapper([vel_c, acc_c], path, path_dist)
ro_solver = cvxpyWrapper([vel_c, robust_acc_c], path, path_dist)
elif solver_name == "ECOS":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
solver = cvxpyWrapper([vel_c, acc_c], path, path_dist)
ro_solver = ecosWrapper([vel_c, robust_acc_c], path, path_dist)
else:
assert False
xmin, xmax = x_ineq
xnext_min = 0
xnext_max = 1
result = solver.solve_stagewise_optim(i, H, g, xmin, xmax, xnext_min,
xnext_max)
ro_result = ro_solver.solve_stagewise_optim(i, H, g, xmin, xmax, xnext_min,
xnext_max)
np.testing.assert_allclose(result, ro_result, atol=1e-4)
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":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
solver = cvxpyWrapper(constraints, path, path_discretization)
elif solver_name == 'qpOASES':
from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper
solver = qpOASESSolverWrapper(constraints, path, path_discretization)
elif solver_name == 'hotqpOASES':
from toppra.solverwrapper.hot_qpoases_solverwrapper import hotqpOASESSolverWrapper
solver = hotqpOASESSolverWrapper(constraints, path,
path_discretization)
elif solver_name == 'ecos':
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
solver = ecosWrapper(constraints, path, path_discretization)
elif solver_name == 'seidel':
from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper
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 __init__(self,
constraint_list,
path,
gridpoints=None,
solver_wrapper=None):
super(ReachabilityAlgorithm, self).__init__(constraint_list,
path,
gridpoints=gridpoints)
logger.debug("Checking supplied constraints.")
has_conic = False
for c in constraint_list:
if c.get_constraint_type() == ConstraintType.CanonicalConic:
has_conic = True
if solver_wrapper is None:
logger.debug(
"Solver wrapper not supplied. Choose solver wrapper automatically!"
)
if has_conic:
solver_wrapper = "ecos"
else:
solver_wrapper = "qpOASES"
logger.debug("Select solver {:}".format(solver_wrapper))
else:
if has_conic:
assert solver_wrapper.lower() in [
'cvxpy', 'ecos'
], "Problem has conic constraints, solver {:} is not suitable".format(
solver_wrapper)
else:
assert solver_wrapper.lower() in [
'cvxpy', 'qpoases', 'ecos', 'hotqpoases', 'seidel'
], "Solver {:} not found".format(solver_wrapper)
# Select
if solver_wrapper.lower() == "cvxpy":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
self.solver_wrapper = cvxpyWrapper(self.constraints, self.path,
self.gridpoints)
elif solver_wrapper.lower() == "qpoases":
from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper
self.solver_wrapper = qpOASESSolverWrapper(self.constraints,
self.path,
self.gridpoints)
elif solver_wrapper.lower() == "hotqpoases":
from toppra.solverwrapper.hot_qpoases_solverwrapper import hotqpOASESSolverWrapper
self.solver_wrapper = hotqpOASESSolverWrapper(
self.constraints, self.path, self.gridpoints)
elif solver_wrapper.lower() == "ecos":
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
self.solver_wrapper = ecosWrapper(self.constraints, self.path,
self.gridpoints)
elif solver_wrapper.lower() == "seidel":
from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper
self.solver_wrapper = seidelWrapper(self.constraints, self.path,
self.gridpoints)
else:
raise NotImplementedError(
"Solver wrapper {:} not found!".format(solver_wrapper))
def test_vel_robust_accel(vel_accel_robustaccel, path, solver_name, i, H, g, x_ineq):
"Case 1: only velocity and robust acceleration constraints. Only linear objective."
vel_c, _, robust_acc_c = vel_accel_robustaccel
path_dist = np.linspace(0, path.duration, 10 + 1)
if solver_name == "cvxpy":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
solver = cvxpyWrapper([vel_c, robust_acc_c], path, path_dist)
elif solver_name == "ECOS":
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
solver = ecosWrapper([vel_c, robust_acc_c], path, path_dist)
else:
assert False
xmin, xmax = x_ineq
xnext_min = 0
xnext_max = 1
# Results from solverwrapper to test
result = solver.solve_stagewise_optim(i, H, g, xmin, xmax, xnext_min, xnext_max)
# Results from cvxpy, used as the actual, desired values
ux = cvxpy.Variable(2)
u = ux[0]
x = ux[1]
_, _, _, _, _, _, xbound = vel_c.compute_constraint_params(path, path_dist, 1.0)
a, b, c, P, _, _ = robust_acc_c.compute_constraint_params(path, path_dist, 1.0)
Di = path_dist[i + 1] - path_dist[i]
cvx_constraints = [
xbound[i, 0] <= x, x <= xbound[i, 1],
x + u * 2 * Di <= xnext_max,
x + u * 2 * Di >= xnext_min,
]
for j in range(a.shape[1]):
cvx_constraints.append(
a[i, j] * u + b[i, j] * x + c[i, j]
+ cvxpy.norm(P[i, j].T[:, :2] * ux + P[i, j].T[:, 2]) <= 0
)
if not np.isnan(xmin):
cvx_constraints.append(x <= xmax)
cvx_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, cvx_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()
np.testing.assert_allclose(
result.flatten(), actual.flatten(), atol=5e-3, rtol=1e-5)
# X must be non-negative, always
assert result[1] >= 0
else:
assert np.all(np.isnan(result))
def test_linear_constraints_only(vel_accel_robustaccel, path, i, g, x_ineq):
"Only canonical linear constraints."
vel_c, acc_c, robust_acc_c = vel_accel_robustaccel
path_dist = np.linspace(0, path.get_duration(), 10 + 1)
solver = ecosWrapper([vel_c, acc_c], path, path_dist)
target_solver = qpOASESSolverWrapper([vel_c, acc_c], path, path_dist)
xmin, xmax = x_ineq
xnext_min = 0
xnext_max = 1
result = solver.solve_stagewise_optim(i, None, g, xmin, xmax, xnext_min,
xnext_max)
target_result = target_solver.solve_stagewise_optim(
i, None, g, xmin, xmax, xnext_min, xnext_max)
np.testing.assert_allclose(result, target_result, atol=1e-5)
def __init__(self,
constraint_list,
path,
gridpoints=None,
solver_wrapper=None,
scaling=1):
super(ReachabilityAlgorithm, self).__init__(constraint_list,
path,
gridpoints=gridpoints)
# Handle gridpoints
if gridpoints is None:
gridpoints = np.linspace(0, path.get_duration(), 100)
logger.info(
"Automatically choose a gridpoint with 100 segments/stages, spaning the input path domain uniformly."
)
if path.get_path_interval()[0] != gridpoints[0]:
logger.fatal("Manually supplied gridpoints does not start from 0.")
raise ValueError("Bad input gridpoints.")
if path.get_path_interval()[1] != gridpoints[-1]:
logger.fatal("Manually supplied gridpoints have endpoint "
"different from input path duration.")
raise ValueError("Bad input gridpoints.")
self.gridpoints = np.array(gridpoints)
self._N = len(
gridpoints) - 1 # Number of stages. Number of point is _N + 1
for i in range(self._N):
if gridpoints[i + 1] <= gridpoints[i]:
logger.fatal(
"Input gridpoints are not monotonically increasing.")
raise ValueError("Bad input gridpoints.")
# path scaling (for numerical stability)
if scaling < 0: # automatic scaling factor selection
# sample a few gradient and compute the average derivatives
qs_sam = path.evald(np.linspace(0, 1, 5) * path.get_duration())
qs_average = np.sum(np.abs(qs_sam)) / path.get_dof() / 5
scaling = np.sqrt(qs_average)
logger.info("[auto-scaling] Average path derivative: {:}".format(
qs_average))
logger.info(
"[auto-scaling] Selected scaling factor: {:}".format(scaling))
else:
logger.info("Scaling factor: {:f}".format(scaling))
# NOTE: Scaling `gridpoints` making the two endpoints different from the domain of the given path
# signal to the lower level solver wrapper that it has to scale the problem. The solver
# wrapper will simply use
# scaling = self.gridpoints[-1] / path.duration
self.gridpoints = self.gridpoints * scaling
# Check for conic constraints
has_conic = False
for c in constraint_list:
if c.get_constraint_type() == ConstraintType.CanonicalConic:
has_conic = True
# Select solver wrapper automatically
if solver_wrapper is None:
logger.debug(
"Solver wrapper not supplied. Choose solver wrapper automatically!"
)
if has_conic:
solver_wrapper = "ecos"
else:
solver_wrapper = "qpOASES"
logger.debug("Select solver {:}".format(solver_wrapper))
# Check solver-wrapper suitability
if has_conic:
assert solver_wrapper.lower() in [
'cvxpy', 'ecos'
], "Problem has conic constraints, solver {:} is not suitable".format(
solver_wrapper)
else:
assert solver_wrapper.lower() in [
'cvxpy', 'qpoases', 'ecos', 'hotqpoases', 'seidel'
], "Solver {:} not found".format(solver_wrapper)
if solver_wrapper.lower() == "cvxpy":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
self.solver_wrapper = cvxpyWrapper(self.constraints, self.path,
self.gridpoints)
elif solver_wrapper.lower() == "qpoases":
from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper
self.solver_wrapper = qpOASESSolverWrapper(self.constraints,
self.path,
self.gridpoints)
elif solver_wrapper.lower() == "hotqpoases":
from toppra.solverwrapper.hot_qpoases_solverwrapper import hotqpOASESSolverWrapper
self.solver_wrapper = hotqpOASESSolverWrapper(
self.constraints, self.path, self.gridpoints)
elif solver_wrapper.lower() == "ecos":
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
self.solver_wrapper = ecosWrapper(self.constraints, self.path,
self.gridpoints)
elif solver_wrapper.lower() == "seidel":
from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper
self.solver_wrapper = seidelWrapper(self.constraints, self.path,
self.gridpoints)
else:
raise NotImplementedError(
"Solver wrapper {:} not found!".format(solver_wrapper))
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":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
solver = cvxpyWrapper(constraints, path, path_discretization)
elif solver_name == 'qpOASES':
from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper
solver = qpOASESSolverWrapper(constraints, path, path_discretization)
elif solver_name == 'hotqpOASES':
from toppra.solverwrapper.hot_qpoases_solverwrapper import hotqpOASESSolverWrapper
solver = hotqpOASESSolverWrapper(constraints, path,
path_discretization)
elif solver_name == 'ecos' and H is None:
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
solver = ecosWrapper(constraints, path, path_discretization)
elif solver_name == 'seidel' and H is None:
from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper
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))
def __init__(self,
constraint_list,
path,
gridpoints=None,
solver_wrapper=None,
scaling=1):
super(ReachabilityAlgorithm, self).__init__(constraint_list,
path,
gridpoints=gridpoints)
# path scaling (for numerical stability)
if scaling < 0: # automatic scaling factor selection
# sample a few gradient and compute the average derivatives
qs_sam = path(np.linspace(0, 1, 5) * path.duration, 1)
qs_average = np.sum(np.abs(qs_sam)) / path.dof / 5
scaling = np.sqrt(qs_average)
logger.info("[auto-scaling] Average path derivative: {:}".format(
qs_average))
logger.info(
"[auto-scaling] Selected scaling factor: {:}".format(scaling))
else:
logger.info("Scaling factor: {:f}".format(scaling))
# NOTE: Scaling `gridpoints` making the two endpoints different from the domain of the given path
# signal to the lower level solver wrapper that it has to scale the problem. The solver
# wrapper will simply use
# scaling = self.gridpoints[-1] / path.duration
self.gridpoints = self.gridpoints * scaling
# Check for conic constraints
has_conic = False
for c in constraint_list:
if c.get_constraint_type() == ConstraintType.CanonicalConic:
has_conic = True
# Select solver wrapper automatically
available_solvers = toppra.solverwrapper.available_solvers()
if solver_wrapper is None:
logger.info(
"Solver wrapper not supplied. Choose solver wrapper automatically!"
)
if has_conic:
if not available_solvers["ecos"]:
raise exceptions.ToppraError(
"Solverwrapper not available.")
solver_wrapper = "ecos"
else:
valid_solver = [
solver for solver, avail in available_solvers if avail
]
solver_wrapper = valid_solver[0]
logger.info("Select solver {:}".format(solver_wrapper))
# Check solver-wrapper suitability
if has_conic:
assert solver_wrapper.lower() in [
"cvxpy",
"ecos",
], "Problem has conic constraints, solver {:} is not suitable".format(
solver_wrapper)
else:
assert solver_wrapper.lower() in [
"cvxpy",
"qpoases",
"ecos",
"hotqpoases",
"seidel",
], "Solver {:} not found".format(solver_wrapper)
if solver_wrapper.lower() == "cvxpy":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
self.solver_wrapper = cvxpyWrapper(self.constraints, self.path,
self.gridpoints)
elif solver_wrapper.lower() == "qpoases":
from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper
self.solver_wrapper = qpOASESSolverWrapper(self.constraints,
self.path,
self.gridpoints)
elif solver_wrapper.lower() == "hotqpoases":
from toppra.solverwrapper.hot_qpoases_solverwrapper import (
hotqpOASESSolverWrapper, )
self.solver_wrapper = hotqpOASESSolverWrapper(
self.constraints, self.path, self.gridpoints)
elif solver_wrapper.lower() == "ecos":
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
self.solver_wrapper = ecosWrapper(self.constraints, self.path,
self.gridpoints)
elif solver_wrapper.lower() == "seidel":
from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper
self.solver_wrapper = seidelWrapper(self.constraints, self.path,
self.gridpoints)
else:
raise NotImplementedError(
"Solver wrapper {:} not found!".format(solver_wrapper))
def __init__(
self, constraint_list, path, gridpoints=None, solver_wrapper=None, parametrizer=None, **kwargs
):
super(ReachabilityAlgorithm, self).__init__(
constraint_list, path, gridpoints=gridpoints, parametrizer=parametrizer, **kwargs
)
# Check for conic constraints
has_conic = False
for c in constraint_list:
if c.get_constraint_type() == ConstraintType.CanonicalConic:
has_conic = True
# Select solver wrapper automatically
available_solvers = toppra.solverwrapper.available_solvers(output_msg=False)
if solver_wrapper is None:
logger.info(
"Solver wrapper not supplied. Choose solver wrapper automatically!"
)
if has_conic:
if not available_solvers["ecos"]:
raise exceptions.ToppraError("Solverwrapper not available.")
solver_wrapper = "ecos"
else:
valid_solver = [solver for solver, avail in available_solvers if avail]
solver_wrapper = valid_solver[0]
logger.info("Select solver {:}".format(solver_wrapper))
# Check solver-wrapper suitability
if has_conic:
assert solver_wrapper.lower() in [
"cvxpy",
"ecos",
], "Problem has conic constraints, solver {:} is not suitable".format(
solver_wrapper
)
else:
assert solver_wrapper.lower() in [
"cvxpy",
"qpoases",
"ecos",
"hotqpoases",
"seidel",
], "Solver {:} not found".format(solver_wrapper)
if solver_wrapper.lower() == "cvxpy":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
self.solver_wrapper = cvxpyWrapper(
self.constraints, self.path, self.gridpoints
)
elif solver_wrapper.lower() == "qpoases":
from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper
self.solver_wrapper = qpOASESSolverWrapper(
self.constraints, self.path, self.gridpoints
)
elif solver_wrapper.lower() == "hotqpoases":
from toppra.solverwrapper.hot_qpoases_solverwrapper import (
hotqpOASESSolverWrapper,
)
self.solver_wrapper = hotqpOASESSolverWrapper(
self.constraints, self.path, self.gridpoints
)
elif solver_wrapper.lower() == "ecos":
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
self.solver_wrapper = ecosWrapper(
self.constraints, self.path, self.gridpoints
)
elif solver_wrapper.lower() == "seidel":
from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper
self.solver_wrapper = seidelWrapper(
self.constraints, self.path, self.gridpoints
)
else:
raise NotImplementedError(
"Solver wrapper {:} not found!".format(solver_wrapper)
)
def __init__(self,
constraint_list,
path,
gridpoints=None,
solver_wrapper=None,
scaling=1):
super(ReachabilityAlgorithm, self).__init__(constraint_list,
path,
gridpoints=gridpoints)
# Handle gridpoints
if gridpoints is None:
gridpoints = interpolator.propose_gridpoints(
path, max_err_threshold=1e-3)
logger.info(
"No gridpoint specified. Automatically choose a gridpoint. See `propose_gridpoints`."
)
if path.path_interval[0] != gridpoints[0] or path.path_interval[
1] != gridpoints[-1]:
raise ValueError("Invalid manually supplied gridpoints.")
self.gridpoints = np.array(gridpoints)
self._N = len(
gridpoints) - 1 # Number of stages. Number of point is _N + 1
for i in range(self._N):
if gridpoints[i + 1] <= gridpoints[i]:
logger.fatal(
"Input gridpoints are not monotonically increasing.")
raise ValueError("Bad input gridpoints.")
# path scaling (for numerical stability)
if scaling < 0: # automatic scaling factor selection
# sample a few gradient and compute the average derivatives
qs_sam = path(np.linspace(0, 1, 5) * path.duration, 1)
qs_average = np.sum(np.abs(qs_sam)) / path.dof / 5
scaling = np.sqrt(qs_average)
logger.info("[auto-scaling] Average path derivative: {:}".format(
qs_average))
logger.info(
"[auto-scaling] Selected scaling factor: {:}".format(scaling))
else:
logger.info("Scaling factor: {:f}".format(scaling))
# NOTE: Scaling `gridpoints` making the two endpoints different from the domain of the given path
# signal to the lower level solver wrapper that it has to scale the problem. The solver
# wrapper will simply use
# scaling = self.gridpoints[-1] / path.duration
self.gridpoints = self.gridpoints * scaling
# Check for conic constraints
has_conic = False
for c in constraint_list:
if c.get_constraint_type() == ConstraintType.CanonicalConic:
has_conic = True
# Select solver wrapper automatically
available_solvers = toppra.solverwrapper.available_solvers()
if solver_wrapper is None:
logger.info(
"Solver wrapper not supplied. Choose solver wrapper automatically!"
)
if has_conic:
if not available_solvers['ecos']:
raise exceptions.ToppraError(
"Solverwrapper not available.")
solver_wrapper = "ecos"
else:
valid_solver = [
solver for solver, avail in available_solvers if avail
]
solver_wrapper = valid_solver[0]
logger.info("Select solver {:}".format(solver_wrapper))
# Check solver-wrapper suitability
if has_conic:
assert solver_wrapper.lower() in ['cvxpy', 'ecos'], \
"Problem has conic constraints, solver {:} is not suitable".format(solver_wrapper)
else:
assert solver_wrapper.lower() in ['cvxpy', 'qpoases', 'ecos', 'hotqpoases', 'seidel'], \
"Solver {:} not found".format(solver_wrapper)
if solver_wrapper.lower() == "cvxpy":
from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper
self.solver_wrapper = cvxpyWrapper(self.constraints, self.path,
self.gridpoints)
elif solver_wrapper.lower() == "qpoases":
from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper
self.solver_wrapper = qpOASESSolverWrapper(self.constraints,
self.path,
self.gridpoints)
elif solver_wrapper.lower() == "hotqpoases":
from toppra.solverwrapper.hot_qpoases_solverwrapper import hotqpOASESSolverWrapper
self.solver_wrapper = hotqpOASESSolverWrapper(
self.constraints, self.path, self.gridpoints)
elif solver_wrapper.lower() == "ecos":
from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper
self.solver_wrapper = ecosWrapper(self.constraints, self.path,
self.gridpoints)
elif solver_wrapper.lower() == "seidel":
from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper
self.solver_wrapper = seidelWrapper(self.constraints, self.path,
self.gridpoints)
else:
raise NotImplementedError(
"Solver wrapper {:} not found!".format(solver_wrapper))
