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 __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))