diff --git a/src/specmicp_common/CMakeLists.txt b/src/specmicp_common/CMakeLists.txt
index 1e75c16..3e236d7 100644
--- a/src/specmicp_common/CMakeLists.txt
+++ b/src/specmicp_common/CMakeLists.txt
@@ -1,132 +1,133 @@
# Main
# ====
# var to store the files
set(specmicp_common_srcs ""
CACHE INTERNAL "specmicp_common files" FORCE)
set(specmicp_common_headers ""
CACHE INTERNAL "specmicp_common headers" FORCE
)
# macro to add to vars
macro(add_to_main_srcs_list LIST_NAME)
set(tmp "")
foreach (src ${${LIST_NAME}})
list(APPEND tmp ${CMAKE_CURRENT_SOURCE_DIR}/${src})
endforeach(src)
set(specmicp_common_srcs "${specmicp_common_srcs};${tmp}" CACHE INTERNAL
"specmicp common files" FORCE)
endmacro(add_to_main_srcs_list)
macro(add_to_main_headers_list LIST_NAME)
set( tmp "")
foreach(header ${${LIST_NAME}})
LIST(APPEND tmp ${CMAKE_CURRENT_SOURCE_DIR}/${header})
endforeach(header)
set(specmicp_common_headers "${specmicp_common_headers};${tmp}"
CACHE INTERNAL "headers for specmicp common" FORCE)
endmacro(add_to_main_headers_list)
#
set( specmicp_common_main_srcs
dateandtime.cpp
filesystem.cpp
log.cpp
moving_average.cpp
string_algorithms.cpp
timer.cpp
)
add_to_main_srcs_list( specmicp_common_main_srcs )
set( specmicp_common_main_headers
cached_vector.hpp
compat.hpp
dateandtime.hpp
filesystem.hpp
log.hpp
macros.hpp
moving_average.hpp
options_handler.hpp
perfs_handler.hpp
pimpl_ptr.hpp
range_iterator.hpp
range_iterator.inl
scope_guard.hpp
string_algorithms.hpp
timer.hpp
types.hpp
)
add_to_main_headers_list( specmicp_common_main_headers )
INSTALL(FILES ${headers}
DESTINATION ${INCLUDE_INSTALL_DIR}/specmicp_common
)
add_subdirectory(cli)
add_subdirectory(eigen)
add_subdirectory(io)
+add_subdirectory(micpsolver)
add_subdirectory(odeint)
add_subdirectory(physics)
add_subdirectory(plugins)
add_subdirectory(sparse_solvers)
# build library
# =============
set_pgo_flag(${specmicp_common_srcs})
add_library(objspecmicp_common OBJECT
${specmicp_common_srcs}
${specmicp_common_headers}
)
set_property(TARGET objspecmicp_common PROPERTY POSITION_INDEPENDENT_CODE 1)
add_library(specmicp_common SHARED $<TARGET_OBJECTS:objspecmicp_common>)
if (UNIX)
set(specmicp_common_link_libraries dl)
else()
message(FATAL_ERROR "Plugin system only for POSIX at this time !")
endif()
if (HDF5_FOUND)
set(specmicp_common_link_libraries
"${HDF5_LIBRARIES};${specmicp_common_link_libraries}")
endif()
target_link_libraries(specmicp_common ${specmicp_common_link_libraries})
install(TARGETS specmicp_common
LIBRARY DESTINATION ${LIBRARY_INSTALL_DIR}
)
# static libraries
# ----------------
if(SPECMICP_BUILD_STATIC)
add_library(specmicp_common_static STATIC
$<TARGET_OBJECTS:objspecmicp_common>
)
install(TARGETS specmicp_common_static
ARCHIVE DESTINATION ${STATIC_LIBRARY_INSTALL_DIR}
)
else()
add_library(specmicp_common_static EXCLUDE_FROM_ALL STATIC
$<TARGET_OBJECTS:objspecmicp_common>
)
endif()
target_link_libraries(specmicp_common_static ${specmicp_common_link_libraries})
set_target_properties(specmicp_common_static PROPERTIES OUTPUT_NAME specmicp_common)
diff --git a/src/micpsolver/CMakeLists.txt b/src/specmicp_common/micpsolver/CMakeLists.txt
similarity index 53%
rename from src/micpsolver/CMakeLists.txt
rename to src/specmicp_common/micpsolver/CMakeLists.txt
index 73e99ff..6496d52 100644
--- a/src/micpsolver/CMakeLists.txt
+++ b/src/specmicp_common/micpsolver/CMakeLists.txt
@@ -1,29 +1,21 @@
-set(MICPSOLVER_HEADERS
+set(micpsolver_headers
micpsolver_base.hpp
micpsolver_base.inl
micpsolver.hpp
micpsolver.inl
micpsolver_structs.hpp
micpprog.hpp
ncp_function.hpp
estimate_cond_number.hpp
-)
-add_custom_target(micpsolver_incl SOURCES
- micpsolver_base.hpp
- micpsolver_base.inl
- micpsolver.hpp
- micpsolver.inl
micpsolver_min.hpp # to remove at some point
micpsolver_min.inl # to remove at some point
micpsolverold.hpp # to remove at some point
micpsolverold.inl # to remove at some point
- micpsolver_structs.hpp
- micpprog.hpp
- ncp_function.hpp
- estimate_cond_number.hpp
)
-install(FILES ${MICPSOLVER_HEADERS}
- DESTINATION ${INCLUDE_INSTALL_DIR}/micpsolver
+add_to_main_headers_list(micpsolver_headers)
+
+install(FILES ${micpsolver_headers}
+ DESTINATION ${INCLUDE_INSTALL_DIR}/specmicp_common/micpsolver
)
diff --git a/src/micpsolver/estimate_cond_number.hpp b/src/specmicp_common/micpsolver/estimate_cond_number.hpp
similarity index 99%
rename from src/micpsolver/estimate_cond_number.hpp
rename to src/specmicp_common/micpsolver/estimate_cond_number.hpp
index 76f7d69..9bf7d0b 100644
--- a/src/micpsolver/estimate_cond_number.hpp
+++ b/src/specmicp_common/micpsolver/estimate_cond_number.hpp
@@ -1,117 +1,117 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#ifndef SPECMICP_MICPSOLVER_ESTIMATECONDNUMBER_HPP
#define SPECMICP_MICPSOLVER_ESTIMATECONDNUMBER_HPP
-#include <Eigen/Core>
+#include "specmicp_common/types.hpp"
//! \file estimate_cond_number.hpp Estimate the condition number of a matrix
namespace specmicp {
namespace micpsolver {
//! \brief Estimate the condition number of a dense square triangular matrix
//!
//! References :
//! - \cite Cline1979
//! - \cite Hager1984
//!
//! @param tmatrix a triangular view of a dense matrix
//! @param maxit maximum number of iterations done by the algorithm (2 triangular solve by iteration)
//!
template <class MatrixType, unsigned int mode>
double estimate_condition_number(Eigen::TriangularView<MatrixType, mode> tmatrix, int maxit=10)
{
const int n = tmatrix.cols();
Eigen::VectorXd x = 1/n*Eigen::VectorXd::Ones(n);
Eigen::VectorXd y(n);
int cnt = 0;
y = tmatrix.solve(x);
while (cnt < maxit)
{
for (int i=0; i<n; ++i)
{
if (y(i) >= 0) y(i) = 1;
else y(i) = -1;
}
tmatrix.solveInPlace(y);
y.reverseInPlace(); // transpose
int j;
const double maxi = y.array().abs().maxCoeff(&j);
const double gamma = y.dot(x);
if (maxi <= gamma) break; // This is a local maximum
x= Eigen::VectorXd::Zero(n);
x(j) = 1;
y = tmatrix.solve(x);
++cnt;
}
// norm tmatrix
// ------------
double nnorm;
if (mode == Eigen::Lower)
{
nnorm = std::abs(tmatrix(0, 0));
for (int i=1; i<n; ++i)
{
double normrow = 0;
for (int j=0; j<i+1; ++j)
{
normrow += std::abs(tmatrix(i, j));
}
nnorm = std::max(nnorm, normrow);
}
}
else
{
nnorm = std::abs(tmatrix(n-1, n-1));
for (int i=n-2; i>-1; --i)
{
double normrow = 0;
for (int j=i; j<n; ++j)
{
normrow += std::abs(tmatrix(i, j));
}
nnorm = std::max(nnorm, normrow);
}
}
// Return ||A||*||A^-1||
return nnorm*x.lpNorm<1>();
}
} // end namespace micpsolver
} // end namespace specmicp
#endif // SPECMICP_MICPSOLVER_ESTIMATECONDNUMBER_HPP
diff --git a/src/micpsolver/micpprog.hpp b/src/specmicp_common/micpsolver/micpprog.hpp
similarity index 99%
rename from src/micpsolver/micpprog.hpp
rename to src/specmicp_common/micpsolver/micpprog.hpp
index a046cd2..dd06163 100644
--- a/src/micpsolver/micpprog.hpp
+++ b/src/specmicp_common/micpsolver/micpprog.hpp
@@ -1,115 +1,115 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#ifndef SPECMICP_MICPSOLVER_MICPPROG_HPP
#define SPECMICP_MICPSOLVER_MICPPROG_HPP
-#include "../types.hpp"
+#include "specmicp_common/types.hpp"
//! \file micpprog.hpp
//! \brief The static interface of a MiCP program
namespace specmicp {
namespace micpsolver {
//! \class MiCPProg
//! \brief A base clase for a MiCP program
//!
//! This class describes a static interface.
//! Any MiCP program should derived from this class and implement its methods
template <class Derived>
class MiCPProg
{
public:
//! \brief Return the derived class
Derived& derived() {return static_cast<Derived*>(*this);}
//! \brief Return the number of variables
//!
//! Sould be implemented by the program
index_t total_variables() {return derived()->total_variables();}
//! \brief Return the number of free variables
//!
//! Sould be implemented by the variables
//!
//! The free variables are the variables not subjected to the complementarity condition.
index_t nb_free_variables() {return derived()->total_variables();}
//! \brief Return the number of constrained variables
//!
//! Not need to implemented this method by the program.
index_t nb_complementarity_variables() {return total_variables() - nb_free_variables();}
//! \brief Return the residuals
//!
//! \param[in] x the variables
//! \param[out] residual the residuals
void get_residuals(const Vector& x,
Vector& residual);
//! \brief Return the jacobian
//!
//! \param[in] x the variables
//! \param[out] jacobian the jacobian
void get_jacobian(Vector& x,
Matrix& jacobian);
//! \brief Called at the beginning of an iteration
//!
//! \param x the variables
//! \param norm_residual norm of the residuals of the previous iteration
//! \return A boolean indicating if the system can have converged
//!
//! Return true by default
bool hook_start_iteration(const Vector& x, scalar_t norm_residual) {return true;}
//! \brief Return the maximum update length that the algorithm can take
//!
//! This is usually used to impose nonnegativity conditions
//!
//! \param x the variables
//! \param update solution of the linear system
//! \return multiplicating factor to scale the update
scalar_t max_lambda(const Vector& x, const Vector& update) {return 1.0;}
//! \brief Return the value used for infinity
static double infinity() {return HUGE_VAL;}
};
} // end namespace micpsolver
} // end namespace specmicp
#endif // SPECMICP_MICPSOLVER_MICPPROG_HPP
diff --git a/src/micpsolver/micpsolver.hpp b/src/specmicp_common/micpsolver/micpsolver.hpp
similarity index 99%
rename from src/micpsolver/micpsolver.hpp
rename to src/specmicp_common/micpsolver/micpsolver.hpp
index 4fe91a5..a2242db 100644
--- a/src/micpsolver/micpsolver.hpp
+++ b/src/specmicp_common/micpsolver/micpsolver.hpp
@@ -1,246 +1,246 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#ifndef SPECMIC_MICPSOLVER_MICPSOLVER_HPP
#define SPECMIC_MICPSOLVER_MICPSOLVER_HPP
-#include "../types.hpp"
+#include "specmicp_common/types.hpp"
#include "micpsolver_base.hpp"
#include "ncp_function.hpp"
//! \file micpsolver.hpp
//! \brief The MiCP solver
namespace specmicp {
//! \namespace specmicp::micpsolver
//! \brief the MiCP Solver(s) and related classes
namespace micpsolver {
//! \class MiCPSolver
//! \brief The MiCP Solver
//!
//! ### Mathematics
//!
//! Solve
//! - \f$ G(u, v) = 0\f$
//! - \f$ 0 \leq v \perp H(u,v) \geq 0 \f$
//!
//! Using the penalized Fisher-Burmeister C-function.
//! This is a semismooth Newton solver with many features including :
//!
//! - Non-monotone linesearch
//! - Scaling of the jacobian
//! - Check of the condition number of the jacobian
//! - Check of the descent directions
//! - Crashing
//! - ...
//!
//! References :
//! - Munson et al (2001) \cite Munson2001
//! - Facchinei and Pang (2003) \cite Facchinei2003
//!
//! ### Usage
//!
//! The options to customize the solver are contained in the struct
//! specmicp::micpsolver::MiCPSolverOptions .
//!
//! \tparam program_t a subclass of MiCPProg
//!
//! To separate the program and the solver, the program is passed as a shared_ptr.
//! The MiCPSolver is implemented using the CRTP pattern.
//! The advantage is that we have compile time polymorphism instead of
//! the runtime polymorphism when using the virtual functions.
//! I am not sure of the effectiveness of this method (I have not run any benchmarks)
//! but since the solver is destined to be called many times any small gain is good to have.
//! Also, more compile-time optimization is possible (especially inlining).
//! Anyway, that was fun to code...
//!
//! \sa specmicp::micpsolver::MiCPProgram
template <class program_t>
class MiCPSolver: public MiCPSolverBaseProgram<program_t>
{
public:
using base = MiCPSolverBaseProgram<program_t>;
using base::get_options;
using base::get_program;
using base::get_perfs;
using base::get_neq;
using base::get_neq_free;
//! \brief Constructor
//!
//! \param prog smart pointer toward an instance of a program_t to solve
MiCPSolver(std::shared_ptr<program_t> prog):
MiCPSolverBaseProgram<program_t>(prog),
m_residuals(prog->total_variables()),
m_phi_residuals(Vector::Zero(prog->total_variables())),
m_jacobian(prog->total_variables(),prog ->total_variables())
{}
// Merit function
// ##############
// Residuals and jacobian
// ----------------------
//! \brief Compute the residuals, use internal storage
//!
//! \param[in] x the variables
void compute_residuals(const Vector& x)
{
base::compute_residuals(x, m_residuals);
}
//! \brief Compute the jacobian
//!
//! Assumes that the residual have been computed before
//!
//! \param[in] x the variables
void compute_jacobian(Vector& x)
{
base::compute_jacobian(x, m_jacobian);
}
//! \brief Reformulation of the residuals
//!
//! Reformulate the problem - assumes that r, the residual, has been computed before
//!
//! \param[in] x the variables
//! \param[in] r the residuals
//! \param[out] r_phi a vector of size neq, which will contain the reformulated residuals
void reformulate_residuals(const Vector& x,
const Vector& r,
Vector& r_phi);
//! \brief Reformulation of the residuals *inplace*
//!
//! \param[in] x the variables
//! \param[in,out] r the residual, will contain the reformulated residuals
void reformulate_residuals_inplace(const Vector &x,
Vector &r);
//! \brief Reformulation of the jacobian
//!
//! r is the original vector of residuals (not reformulated)
void reformulate_jacobian(const Vector& x) {
base::reformulate_jacobian_cck(x, m_residuals, m_jacobian);
}
// Algorithm
// #########
//! \brief Solver the program using x as initial guess
//!
//! \param[in,out] x the initial guess, as output contains the solution (from the last iteration)
MiCPSolverReturnCode solve(Vector& x);
//! \brief Setup the residuals
//!
//! \param[in] x the current solution
void setup_residuals(const Eigen::VectorXd& x) {
compute_residuals(x);
reformulate_residuals(x, m_residuals, m_phi_residuals);
}
//! \brief Setup the jacobian
//!
//! \param[in] x the current solution
void setup_jacobian(Eigen::VectorXd& x) {
compute_jacobian(x);
reformulate_jacobian(x);
m_grad_phi = m_jacobian.transpose() * m_phi_residuals;
}
//! \brief Solve the Newton system
//!
//! The system will be scaled before being solved, may be expensive and not necessary.
//! Do disable the scaling, disable the corresponding options in specmicp::MiCPSolver::MiCPSolverOptions.
//! It assumes that the Newton system has been formed
//!
//! \param[out] update the update to apply to the solution
//!
//! \sa search_direction_calculation_no_scaling.
MiCPSolverReturnCode search_direction_calculation(Vector& update);
//! \brief Solve the Newton system - does not scale the jacobian
//!
//! The system will not be scaled.
//! It assumes that the Newton system has been formed
//!
//! \param[out] update the update to apply to the solution
//!
//! \sa search_direction_scaling
MiCPSolverReturnCode search_direction_calculation_no_scaling(Vector &update);
//! \brief Linesearch
//!
//! It is a backtracking linesearch.
//! If the correct option is set, this is a nonmonotone linesearch.
//!
//! \param[in,out] update the update of the timestep
//! \param[in,out] x the current solution
//!
//! References :
//! - \cite Dennis1983
//! - \cite Munson2001
int linesearch(Eigen::VectorXd& update, Eigen::VectorXd& x);
//! \brief Crashing
//!
//! This function improves, if possible, the initial guess
//!
//! \param[in] x the current solution
//!
//! Reference :
//! - \cite Munson2001
void crashing(Vector &x);
private:
// Residuals and jacobian
Eigen::VectorXd m_residuals; //!< The residuals
Eigen::VectorXd m_phi_residuals; //!< The reformulated residuals
Eigen::VectorXd m_grad_phi; //!< The gradient of the reformulated residuals
Eigen::MatrixXd m_jacobian; //!< The jacobian
std::vector<scalar_t> m_max_merits; //!< Contains the m best value of the merit function
bool m_gradient_step_taken {false}; //!< True if the update was computed using the gradient
};
} // end namespace micpsolver
} // end namespace specmicp
// ###############//
// Implementation //
// ###############//
#include "micpsolver.inl"
#endif // SPECMIC_MICPSOLVER_MICPSOLVER_HPP
diff --git a/src/micpsolver/micpsolver.inl b/src/specmicp_common/micpsolver/micpsolver.inl
similarity index 99%
rename from src/micpsolver/micpsolver.inl
rename to src/specmicp_common/micpsolver/micpsolver.inl
index 2202383..a21bc4e 100644
--- a/src/micpsolver/micpsolver.inl
+++ b/src/specmicp_common/micpsolver/micpsolver.inl
@@ -1,450 +1,451 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#ifndef SPECMIC_MICPSOLVER_MICPSOLVER_HPP
#include "micpsolver.hpp" // for syntaxic coloration...
#endif // SPECMIC_MICPSOLVER_MICPSOLVER_HPP
#include <Eigen/QR>
#include "estimate_cond_number.hpp"
-#include "../utils/log.hpp"
+
+#include "specmicp_common/log.hpp"
//! \file micpsolver.inl implementation of the MiCP solver
namespace specmicp {
namespace micpsolver {
// Main algorithm
// ##############
template <class program_t>
MiCPSolverReturnCode MiCPSolver<program_t>::solve(Vector &x)
{
int cnt = 0;
if (get_options().use_crashing) crashing(x);
else setup_residuals(x);
MiCPSolverReturnCode retcode = MiCPSolverReturnCode::NotConvergedYet;
Eigen::VectorXd update(get_neq());
while (retcode == MiCPSolverReturnCode::NotConvergedYet)
{
DEBUG << "Iteration : " << cnt;
// (S.0) Initialization of the iteration
// this is a hook for simultaneous fixed-point iterations
// implemented to solve the non-ideality model
bool may_have_converged = get_program()->hook_start_iteration(x, m_phi_residuals.norm());
// (S.1) Check the convergence
setup_residuals(x);
get_perfs().current_residual = m_phi_residuals.norm();
DEBUG << "Norm residuals : " << get_perfs().current_residual;
retcode = base::check_convergence(cnt, update, x, m_phi_residuals, may_have_converged);
get_perfs().return_code = retcode;
if (retcode != MiCPSolverReturnCode::NotConvergedYet) break;
++cnt;
// (S.2) Compute the jacobian and solve the linear problem
setup_jacobian(x);
if(get_options().use_scaling)
search_direction_calculation(update);
else
search_direction_calculation_no_scaling(update);
// (S.3) Linesearch
int termcode = linesearch(update, x);
if (termcode != 0) retcode = MiCPSolverReturnCode::LinesearchFailure;
get_perfs().return_code = retcode;
get_perfs().current_update = update.norm();
DEBUG << "Return LineSearch : " << termcode;
base::projection(x); // project x to the positive quadrant
get_perfs().nb_iterations = cnt;
}
return retcode;
}
template <class program_t>
MiCPSolverReturnCode MiCPSolver<program_t>::search_direction_calculation(Eigen::VectorXd& update)
{
Vector rscaler(Vector::Ones(m_jacobian.cols()));
Vector cscaler(Vector::Ones(m_jacobian.rows()));
base::scaling_jacobian(m_jacobian, m_phi_residuals, rscaler, cscaler);
m_jacobian = rscaler.asDiagonal() * (m_jacobian) * cscaler.asDiagonal();
Eigen::ColPivHouseholderQR<Matrix> solver(m_jacobian.rows(), m_jacobian.cols()); // it needs to be correctly initialized
m_gradient_step_taken = false;
int m;
for (m=0; m<get_options().max_factorization_step; ++m)
{
const scalar_t lambda = get_options().factor_gradient_search_direction;
solver.compute(m_jacobian);
get_perfs().nb_factorization += 1;
if (solver.info() != Eigen::Success or not solver.isInvertible())
{
DEBUG << "Solver.info : " << solver.info() << " - is invertible : " << solver.isInvertible();
ERROR << "System cannot be solved, we try a perturbation";
m_gradient_step_taken = true;
m_jacobian.diagonal() += lambda*rscaler.cwiseProduct(cscaler);
continue;
}
if (get_options().condition_limit > 0)
{
scalar_t cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
if (cond > get_options().condition_limit)
{
m_gradient_step_taken = true;
m_jacobian.diagonal() += lambda*rscaler.cwiseProduct(cscaler);
continue;
}
}
update = solver.solve(-rscaler.cwiseProduct(m_phi_residuals + m*lambda*m_grad_phi));
update = cscaler.cwiseProduct(update);
if (get_options().factor_descent_condition < 0 ) break; // we have a solution
// else we compute the descent condition
scalar_t descent_cond = m_grad_phi.dot(update);
scalar_t norm_grad = m_grad_phi.norm();
scalar_t norm_update = update.norm();
if ((descent_cond <= -get_options().factor_descent_condition*std::min(
std::pow(norm_update,2),std::pow(norm_update,3)))
and (descent_cond <= -get_options().factor_descent_condition*std::min(
std::pow(norm_grad,2),std::pow(norm_grad,3)))
)
break; // we have a solution !
m_gradient_step_taken = true;
m_jacobian.diagonal() += lambda*rscaler.cwiseProduct(cscaler);
}
DEBUG << "Gradient step : m = " << m;
if (m == get_options().max_factorization_step) {
INFO << "Full gradient step taken !";
update = -m_grad_phi;
}
return MiCPSolverReturnCode::NotConvergedYet;
}
template <class program_t>
MiCPSolverReturnCode MiCPSolver<program_t>::search_direction_calculation_no_scaling(
Vector& update
)
{
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver(m_jacobian.rows(), m_jacobian.cols()); // it needs to be correctly initialized
// if everything is ok, this is the only decomposition
m_gradient_step_taken = false;
int m; // this is an index that increase the contribution of the gradient in the solution
// the pure Newton solution is when m=0
for (m=0; m<get_options().max_factorization_step; ++m)
{
const scalar_t lambda = get_options().factor_gradient_search_direction;
solver.compute(m_jacobian); // we need to recompute it (?)
get_perfs().nb_factorization += 1;
// Check that the decomposition was ok
if (solver.info() != Eigen::Success or not solver.isInvertible())
{
DEBUG << "Solver.info : " << solver.info() << " (should be " << Eigen::Success
<< ") - is invertible : " << solver.isInvertible() << " (sould be " << true << ")";
ERROR << "System cannot be solved, we try a perturbation";
m_gradient_step_taken = true;
m_jacobian.diagonal() += Eigen::VectorXd::Constant(m_jacobian.rows(), lambda);
continue;
}
// Estimate the condition number
if (get_options().condition_limit > 0)
{
scalar_t cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
if (cond > get_options().condition_limit)
{
// add the perturbation
m_gradient_step_taken = true;
m_jacobian.diagonal() += Eigen::VectorXd::Constant(m_jacobian.rows(), lambda);
}
}
update = solver.solve(-(m_phi_residuals + m*lambda*m_grad_phi));
if (get_options().factor_descent_condition < 0 ) break; // we have a solution
scalar_t descent_cond = m_grad_phi.dot(update);
scalar_t norm_grad = m_grad_phi.norm();
scalar_t norm_update = update.norm();
if ( (descent_cond <= -get_options().factor_descent_condition*std::min(
std::pow(norm_update,2),std::pow(norm_update,3)))
and (descent_cond <= -get_options().factor_descent_condition*std::min(
std::pow(norm_grad,2),std::pow(norm_grad,3)))
)
break; // we have a solution !
// add the perturbation
m_gradient_step_taken = true;
m_jacobian.diagonal() += Eigen::VectorXd::Constant(m_jacobian.rows(), lambda);
}
DEBUG << "Gradient step : m = " << m;
if (m_gradient_step_taken && m == get_options().max_factorization_step) {
INFO << "Full gradient step taken !";
update = -m_grad_phi;
}
return MiCPSolverReturnCode::NotConvergedYet;
}
template <class program_t>
void MiCPSolver<program_t>::crashing(Vector& x)
{
// steepest descent direction algorithm
DEBUG << "Crashing ";
const scalar_t beta = 0.5;
const scalar_t sigma = 1e-5;
int cnt = 0;
while (cnt < 10)
{
setup_residuals(x);
setup_jacobian(x);
m_grad_phi = m_jacobian.transpose()*m_phi_residuals;
Vector xp(get_neq());
int l=0;
const int maxl = 10;
while (l<maxl)
{
xp = x - std::pow(beta, l)*m_grad_phi;
for (int i=get_neq_free(); i<get_neq(); ++i)
{
if (xp(i) < 0) xp(i) = 0;
}
Vector new_res(get_neq());
base::compute_residuals(x, new_res);
reformulate_residuals_inplace(x, new_res);
scalar_t test = 0.5*(new_res.squaredNorm() - m_phi_residuals.squaredNorm()) + sigma*m_grad_phi.dot(x - xp);
if (test <= 0) break;
++l;
}
if (l == maxl) break;
x = xp;
++cnt;
}
get_perfs().nb_crashing_iterations = cnt;
DEBUG << "Crashing iterations : " << cnt;
m_max_merits.reserve(4);
SPAM << "Solution after crashing \n ------ \n " << x << "\n ----- \n";
}
// Others
// ######
template <class program_t>
void MiCPSolver<program_t>::reformulate_residuals(
const Vector &x,
const Vector &r,
Vector &r_phi
)
{
// reformulation with copy from r to r_phi
r_phi.resizeLike(r);
r_phi.block(0, 0, get_neq_free(), 1) = r.block(0, 0, get_neq_free(), 1);
for (index_t i = get_neq_free(); i<get_neq(); ++i)
{
r_phi(i) = penalized_fisher_burmeister(x(i), r(i), get_options().penalization_factor);
}
}
template <class program_t>
void MiCPSolver<program_t>::reformulate_residuals_inplace(
const Vector& x,
Vector& r
)
{
for (index_t i = get_neq_free(); i<get_neq(); ++i)
{
r(i) = penalized_fisher_burmeister(x(i), r(i), get_options().penalization_factor);
}
}
template <class program_t>
int MiCPSolver<program_t>::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x)
{
// References
// ----------
// - Algo A6.3.1 : Dennis and Schnabel (1983)
// - Munson et al. (2001)
// - Facchinei (2003)
// - Nocedal & Wrigth (2006)
DEBUG << "Linesearch";
Eigen::VectorXd xp(get_neq());
Eigen::VectorXd new_res(get_neq());
double fcp;
get_perfs().max_taken = false;
int retcode = 2;
const double alpha = 1e-4;
double newtlen = base::is_step_too_long(p);
double init_slope = m_grad_phi.dot(p);
double rellength = std::abs(p(0));
for (int i=1; i<get_neq(); ++i)
{
rellength = std::max(rellength, std::abs(p(i)));
}
double minlambda = get_options().steptol / rellength;
double lambda = get_program()->max_lambda(x, p);
double lambda_prev = lambda;
// non monotone linesearch
// =======================
double merit_value = 0.5*m_phi_residuals.squaredNorm();
// new residual
xp = x + lambda*p;
base::compute_residuals(xp, new_res);
reformulate_residuals_inplace(xp, new_res);
fcp = 0.5*new_res.squaredNorm();
// Skip linesearch if enough progress is done
// ------------------------------------------
if (fcp < get_options().coeff_accept_newton_step *merit_value)
{
if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value;
else m_max_merits.push_back(merit_value);
x = xp;
return 0;
}
// Select the merit value of reference
// -----------------------------------
if (get_options().non_monotone_linesearch)
{
double mmax = merit_value;
if (m_max_merits.size() > 0)
{
mmax = m_max_merits[m_max_merits.size()-1];
// check for cycling
// - - - - - - - - -
if ( m_max_merits.size() ==4
&& std::fabs(mmax - merit_value) < get_options().threshold_cycling_linesearch*get_options().fvectol)
{
//std::cout << merit_value << std::endl;
WARNING << "Cycling has been detected by the linesearch - Taking the full Newton step";
x = xp;
p = lambda*p;
return 3;
}
}
if (m_max_merits.size() < 4)
{
m_max_merits.push_back(merit_value);
if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4;
}
else if (merit_value < mmax)
{
m_max_merits[3] = merit_value;
merit_value = mmax;
}
if (m_gradient_step_taken)
{
merit_value *= 100;
}
}
// The linesearch
// --------------
double fc = merit_value;
double fcp_prev;
int cnt = 0;
do
{
fcp = 0.5*new_res.squaredNorm();
if (fcp <= fc - std::min(-alpha*lambda*init_slope,(1-alpha)*fc)) //pg760 Fachinei2003
{
retcode = 0;
if (lambda ==1 and (newtlen > 0.99 * get_options().maxstep)) {
get_perfs().max_taken = true;
}
break;
}
else if (lambda < minlambda)
{
ERROR << "Linesearch cannot find a solution bigger than the minimal step";
retcode = 1;
break;
}
else
{
// Select a new step length
// - - - - - - - - - - - -
double lambdatmp;
if (cnt == 0) { // only a quadratic at the first
lambdatmp = - init_slope / (2*(fcp - fc -init_slope));
}
else
{
const double factor = 1 /(lambda - lambda_prev);
const double x1 = fcp - fc - lambda*init_slope;
const double x2 = fcp_prev - fc - lambda_prev*init_slope;
const double a = factor * ( x1/(lambda*lambda) - x2/(lambda_prev*lambda_prev));
const double b = factor * ( -x1*lambda_prev/(lambda*lambda) + x2*lambda/(lambda_prev*lambda_prev));
if (a == 0)
{ // cubic interpolation is in fact a quadratic interpolation
lambdatmp = - init_slope/(2*b);
}
else
{
const double disc = b*b-3*a*init_slope;
lambdatmp = (-b+std::sqrt(disc))/(3*a);
}
if (lambdatmp > 0.5*lambda ) lambdatmp = 0.5*lambda;
}
lambda_prev = lambda;
fcp_prev = fcp;
if (lambdatmp < 0.1*lambda) {
lambda = 0.1 * lambda;
} else {
lambda = lambdatmp;
}
if (not std::isfinite(lambda))
{
ERROR << "Lambda is non finite in micpsolver linesearch - we stop";
return 3;
}
}
xp = x + lambda*p;
base::compute_residuals(xp, new_res);
reformulate_residuals_inplace(xp, new_res);
++cnt;
} while(retcode == 2 and cnt < 100);
DEBUG << "Lambda : " << lambda;
if (cnt == 100)
{
ERROR << "Too much linesearch iterations ! We stop";
}
x = xp;
p = lambda*p;
return retcode;
}
} // end namespace micpsolver
} // end namespace specmicp
diff --git a/src/micpsolver/micpsolver_base.hpp b/src/specmicp_common/micpsolver/micpsolver_base.hpp
similarity index 97%
rename from src/micpsolver/micpsolver_base.hpp
rename to src/specmicp_common/micpsolver/micpsolver_base.hpp
index 3497c25..b5da199 100644
--- a/src/micpsolver/micpsolver_base.hpp
+++ b/src/specmicp_common/micpsolver/micpsolver_base.hpp
@@ -1,192 +1,194 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#ifndef SPECMICP_MICPSOLVER_MICPSOLVERBASE_HPP
#define SPECMICP_MICPSOLVER_MICPSOLVERBASE_HPP
//! \file micpsolver_base.hpp
//! \brief Base class for micpsolver
#include <memory>
-#include "../types.hpp"
+
#include "micpsolver_structs.hpp"
-#include "../utils/log.hpp"
-#include "../utils/options_handler.hpp"
-#include "../utils/perfs_handler.hpp"
+#include "specmicp_common/types.hpp"
+#include "specmicp_common/log.hpp"
+
+#include "specmicp_common/options_handler.hpp"
+#include "specmicp_common/perfs_handler.hpp"
namespace specmicp {
namespace micpsolver {
//! \brief Base class for an MiCP solver
//!
//! Handle the program, options and performance
//!
template <class program_t>
class MiCPSolverBaseProgram:
public OptionsHandler<MiCPSolverOptions>,
public PerformanceHandler<MiCPPerformance>
{
public:
using program_ptr = std::shared_ptr<program_t>;
using OptionsHandler<MiCPSolverOptions>::get_options;
using PerformanceHandler<MiCPPerformance>::get_perfs;
MiCPSolverBaseProgram():
OptionsHandler<MiCPSolverOptions>(),
PerformanceHandler<MiCPPerformance>()
{}
//! \brief Constructor
//!
//! \param prog shared_ptr to the program
MiCPSolverBaseProgram(program_ptr prog):
OptionsHandler<MiCPSolverOptions>(),
PerformanceHandler<MiCPPerformance>(),
m_program(prog)
{}
//! \brief Return the program
//!
//! This is due to the templating, allow to do get_program()->whatever();
program_ptr& get_program() {return m_program;}
//! \brief Return the number of equations
index_t get_neq() const {return m_program->total_variables();}
//! \brief Return the number of equations corresponding to the free variables (size of G)
index_t get_neq_free() const {return m_program->nb_free_variables();}
//! \brief Compute the residuals, store it in r
//!
//! \param[in] x the variables
//! \param[out] r vector to store the residuals (must be of the same size as x)
void compute_residuals(const Vector& x, Vector& r)
{
m_program->get_residuals(x, r);
get_perfs().nb_call_residuals += 1;
}
//! \brief Compute the jacobian
//!
//! Assumes that the residual have been computed before
//!
//! \param[in] x the variables
//! \param[out] jacobian the jacobian
void compute_jacobian(Vector& x, Matrix& jacobian)
{
m_program->get_jacobian(x, jacobian);
get_perfs().nb_call_jacobian += 1;
}
//! \brief Compute the factors to scale the jacobian
//!
//! \param[in] jacobian the jacobian to scale (from the reformulated problem)
//! \param[in] residuals the residuals corresponding to the jacobian
//! \param[out] rscaler scaling factors of the rows
//! \param[out] cscaler scaling factors of the columns
void scaling_jacobian(
const Matrix& jacobian,
const Vector& residuals,
Vector& rscaler,
Vector& cscaler);
//! \brief Check for convergence
//!
//! \param nb_iterations the number of iterations
//! \param update the update taken at the previous iteration
//! \param solution the current solution
//! \param residuals current residuals
//! \param may_have_converged a boolean given by the user to indicates if the problem has converged yet
//! \return a MiCPSolverReturnCode describing the state of the algorithm
MiCPSolverReturnCode check_convergence(int nb_iterations,
const Vector& update,
const Vector& solution,
const Vector &residuals,
bool may_have_converged = true);
//! \brief Reformulate the jacobian using the cck function
//!
//! \param x the variables
//! \param r the residuals
//! \param jacobian the jacobian
void reformulate_jacobian_cck(
const Vector& x,
const Vector& r,
Matrix& jacobian
);
//! \brief Compute the norm of the update
//!
//! \tparam p order of the norm (1, 2, ..., Eigen::Infinity)
//! \param update the solution of the linear system
//! \param solution the solution (variables)
template <int p>
double norm_update(const Vector& update,
const Vector& solution) const {
return (update.array().abs()/(solution.array().max(1))
).matrix().template lpNorm<p>();
}
//! \brief Project variables on the feasible set
//!
//! \param x the variables
void projection(Vector& x);
//! \brief Return the step corrected step length if it is too long
//!
//! \param update the solution of the linear system
scalar_t is_step_too_long(Vector& update);
protected:
//! \brief Set the program
//!
//! \param theprogram shared_ptr to the program
program_ptr& set_program(program_ptr theprogram) {
m_program = theprogram;
}
private:
program_ptr m_program;
};
} // end namespace micpsolver
} // end namespace specmicp
// ----------------------------------- //
// Implementation //
// ----------------------------------- //
#include "micpsolver_base.inl"
#endif // SPECMICP_MICPSOLVER_MICPSOLVERBASE_HPP
diff --git a/src/micpsolver/micpsolver_base.inl b/src/specmicp_common/micpsolver/micpsolver_base.inl
similarity index 100%
rename from src/micpsolver/micpsolver_base.inl
rename to src/specmicp_common/micpsolver/micpsolver_base.inl
diff --git a/src/micpsolver/micpsolver_min.hpp b/src/specmicp_common/micpsolver/micpsolver_min.hpp
similarity index 100%
rename from src/micpsolver/micpsolver_min.hpp
rename to src/specmicp_common/micpsolver/micpsolver_min.hpp
diff --git a/src/micpsolver/micpsolver_min.inl b/src/specmicp_common/micpsolver/micpsolver_min.inl
similarity index 99%
rename from src/micpsolver/micpsolver_min.inl
rename to src/specmicp_common/micpsolver/micpsolver_min.inl
index f7778c8..d04c1ec 100644
--- a/src/micpsolver/micpsolver_min.inl
+++ b/src/specmicp_common/micpsolver/micpsolver_min.inl
@@ -1,385 +1,386 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#include "micpsolver_min.hpp" // for syntaxic coloration
-#include "utils/log.hpp"
#include "ncp_function.hpp"
#include "estimate_cond_number.hpp"
+#include "specmicp_common/log.hpp"
+
namespace specmicp {
namespace micpsolver {
template <class program_t>
MiCPSolverReturnCode MiCPSolverMin<program_t>::solve(Eigen::VectorXd& x)
{
int cnt = 0;
MiCPSolverReturnCode retcode = MiCPSolverReturnCode::NotConvergedYet;
Eigen::VectorXd update(Eigen::VectorXd::Zero(get_neq()));
setup_residuals(x);
while (retcode == MiCPSolverReturnCode::NotConvergedYet)
{
DEBUG << "Iteration : " << cnt;
DEBUG << "Solution : \n" << x;
get_program()->hook_start_iteration(x, m_residuals.norm());
setup_residuals(x);
get_perf().current_residual = m_residuals.norm();
SPAM << "Residuals : \n ----- \n" << m_residuals << "\n ----- \n";
retcode = base::check_convergence(cnt, update, x, m_residuals);
get_perf().return_code = retcode;
if (retcode != MiCPSolverReturnCode::NotConvergedYet) break;
++cnt;
get_perf().max_taken = false;
setup_jacobian(x);
search_direction_calculation(x, update);
sanitize(x);
int termcode = linesearch(update, x);
get_perf().current_update = update.norm();
DEBUG << "Return LineSearch : " << termcode;
base::projection(x);
get_perf().nb_iterations = cnt;
}
return retcode;
}
template <class program_t>
MiCPSolverReturnCode MiCPSolverMin<program_t>::search_direction_calculation(
Eigen::VectorXd& x,
Eigen::VectorXd& update)
{
Eigen::MatrixXd reduced_jacobian;
Eigen::VectorXd reduced_residual;
reduce_system(x, reduced_jacobian, reduced_residual);
DEBUG << reduced_jacobian;
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver;
m_gradient_step_taken = false;
solver.compute(reduced_jacobian);
get_perf().nb_factorization += 1;
{ // first condition : is the factorization ok ?
if (solver.info() != Eigen::Success or not solver.isInvertible())
{
DEBUG << "Solver.info : " << solver.info() << " - is invertible : " << solver.isInvertible();
m_gradient_step_taken = true;
goto after_second_cond; // jump directly to the gradient step
}
}
{ // second condition : is the condition number ok ?
const double cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
DEBUG << "Condition number : " << cond;
if (cond > get_options().condition_limit)
{
m_gradient_step_taken = true;
}
}
after_second_cond:
// third condition : is the descent condition respected
update = solver.solve(-reduced_residual);
m_grad_phicck = reduced_jacobian.transpose()*reduced_residual;
const double descent_cond = m_grad_phicck.dot(update);
reformulate_result(x, update);
base::reformulate_jacobian_cck(x, m_residuals, m_jacobian);
reformulate_residuals_cck_inplace(x, m_residuals);
m_grad_phicck = m_jacobian.transpose()*m_residuals;
m_grad_phicck(0) = 0;
if (not m_gradient_step_taken)
{
m_newton_length = base::is_step_too_long(update);
// we compute the descent condition
//const double descent_cond = m_grad_phicck.dot(update);
const double norm_update = update.norm();
DEBUG << "grad(phi).dot(update) = " << descent_cond << " compared to : " << (
- get_options().factor_descent_condition * std::pow(norm_update, get_options().power_descent_condition));
if (descent_cond > - get_options().factor_descent_condition * std::pow(norm_update, get_options().power_descent_condition))
{
m_gradient_step_taken = true;
}
}
if (m_gradient_step_taken)
{
INFO << "Full gradient step taken !";
update = - m_grad_phicck;
m_newton_length = base::is_step_too_long(update);
}
return MiCPSolverReturnCode::NotConvergedYet;
}
template <class program_t>
int MiCPSolverMin<program_t>::reduce_system(const Eigen::VectorXd& x,
Eigen::MatrixXd& reduced_jacobian,
Eigen::VectorXd& reduced_residual)
{
reduced_jacobian.resizeLike(m_jacobian); // memory is cheap, we will resize at the end
reduced_jacobian.setZero();
reduced_residual.resizeLike(m_residuals);
// copy identical information
int ideq_reduced = get_neq_free();
reduced_jacobian.block(0, 0, get_neq_free(), get_neq_free()) = m_jacobian.block(0, 0, get_neq_free(), get_neq_free());
// select active degree of freedom
Eigen::VectorXd to_remove(get_neq()-get_neq_free());
for (int dof=get_neq_free(); dof<get_neq(); ++dof)
{
if (x(dof) >= m_residuals(dof))
{
DEBUG << "Mineral to precipitate : " << dof;
reduced_residual(ideq_reduced) = m_residuals(dof);
reduced_jacobian.block(ideq_reduced, 0, 1, get_neq_free()) = m_jacobian.block(dof, 0, 1, get_neq_free());
reduced_jacobian.block(0, ideq_reduced, get_neq_free(), 1) = m_jacobian.block(0, dof, get_neq_free(), 1);
to_remove(dof-get_neq_free()) = 0;
++ideq_reduced;
}
else
{
to_remove(dof-get_neq_free()) = x(dof);
}
}
reduced_residual.block(0, 0, get_neq_free(), 1) -=
m_jacobian.block(0, get_neq_free(), get_neq_free(), get_neq()-get_neq_free())*to_remove;
reduced_jacobian.conservativeResize(ideq_reduced, ideq_reduced);
reduced_residual.conservativeResize(ideq_reduced);
DEBUG << "ideq reduced : " << ideq_reduced;
return ideq_reduced;
}
template <class program_t>
void MiCPSolverMin<program_t>::reformulate_result(const Eigen::VectorXd& x,
Eigen::VectorXd& update)
{
update.conservativeResizeLike(x);
int tot_to_keep = 0;
for (int dof=get_neq_free(); dof<get_neq(); ++dof)
{
if (x(dof) >= m_residuals(dof))
++tot_to_keep;
}
int kept_dof = 1;
for (int dof=get_neq()-1; dof>=get_neq_free(); --dof)
{ // we go backwards to avoid extra copies
if (x(dof) >= m_residuals(dof))
{
update(dof) = update(get_neq_free()+(tot_to_keep-kept_dof));
++kept_dof;
}
else
{
update(dof) = -x(dof);
}
}
}
template <class program_t>
void MiCPSolverMin<program_t>::sanitize(Eigen::VectorXd& x)
{
if (x(0) <=0) x(0) = 1;
for (int dof=get_neq_free(); dof<get_neq(); ++dof)
{
if (x(dof) <=0) {x(dof) =0; DEBUG<< "sanitize dof : " << dof;}
}
}
template <class program_t>
void MiCPSolverMin<program_t>::reformulate_residuals_cck_inplace(const Eigen::VectorXd& x,
Eigen::VectorXd& residuals)
{
for (int i = get_neq_free(); i<get_neq(); ++i)
{
residuals(i) = penalized_fisher_burmeister(x(i), residuals(i), get_options().penalization_factor);
}
}
template <class program_t>
int MiCPSolverMin<program_t>::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x)
{
// Reference Algo A6.3.1 : Dennis and Schnabel (1983)
DEBUG << "Linesearch";
Eigen::VectorXd xp(get_neq());
Eigen::VectorXd new_res(get_neq());
double fcp;
get_perf().max_taken = false;
int retcode = 2;
const double alpha = get_options().factor_descent_condition;
double newtlen = m_newton_length;
//double newtlen = p.norm();
double init_slope = m_grad_phicck.dot(p);
double rellength = std::abs(p(0));
for (int i=1; i<p.rows(); ++i)
{
rellength = std::max(rellength, std::abs(p(i)));
}
double minlambda = get_options().steptol / rellength;
double lambda = get_program()->max_lambda(x, p);
DEBUG << "Initial lambda : " << lambda;
double lambda_prev = lambda;
// non monotone linesearch
//
// - reference : Munson et al. (2001)
// ------------------------------------
double merit_value = 0.5*m_residuals.squaredNorm();
// // new residual
//reformulate_result(x, p);
xp = x + lambda*p;
DEBUG << "update \n" << p <<std::endl;
base::compute_residuals(xp, new_res);
reformulate_residuals_cck_inplace(xp, new_res);
fcp = 0.5*new_res.squaredNorm();
// Skip linesearch if enough progress is done
if (fcp < get_options().coeff_accept_newton_step *merit_value)
{
if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value;
else m_max_merits.push_back(merit_value);
x = xp;
return 0;
}
DEBUG << "Merit value : " << merit_value;
double mmax = merit_value;
if (m_max_merits.size() > 0)
{
mmax = m_max_merits[m_max_merits.size()-1];
}
if (m_max_merits.size() < 4)
{
m_max_merits.push_back(merit_value);
if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4;
}
else if (merit_value < mmax)
{
m_max_merits[3] = merit_value;
merit_value = mmax;
}
if (m_gradient_step_taken)
{
merit_value *= 100;
}
DEBUG << "Merit value used : " << merit_value;
double fc = merit_value;
double fcp_prev;
int cnt = 0;
do
{
DEBUG << "cnt : " << cnt << " - lambda : " << lambda;
DEBUG << "fcp : " << fcp << "\n fc+alin : " << fc+alpha*lambda*init_slope << " # fc : " << fc << std::endl;
if (fcp <= fc + alpha*lambda*init_slope)
{
retcode = 0;
if (lambda ==1 and (newtlen > 0.99 * get_options().maxstep)) {
get_perf().max_taken = true;
}
break;
}
else if (lambda < minlambda)
{
lambda = get_program()->max_lambda(x, p);
xp = x + lambda*p;
retcode = 1;
break;
}
else
{
double lambdatmp;
if (cnt == 0) { // only a quadratic at the first
lambdatmp = - init_slope / (2*(fcp - fc -init_slope));
}
else
{
const double factor = 1 /(lambda - lambda_prev);
const double x1 = fcp - fc - lambda*init_slope;
const double x2 = fcp_prev - fc - lambda_prev*init_slope;
const double a = factor * ( x1/(lambda*lambda) - x2/(lambda_prev*lambda_prev));
const double b = factor * ( -x1*lambda_prev/(lambda*lambda) + x2*lambda/(lambda_prev*lambda_prev));
if (a == 0)
{ // cubic interpolation is in fact a quadratic interpolation
DEBUG << "not disc : " << - init_slope/(2*b);
lambdatmp = - init_slope/(2*b);
}
else
{
const double disc = b*b-3*a*init_slope;
lambdatmp = (-b+std::sqrt(disc))/(3*a);
}
if (lambdatmp > 0.5*lambda ) lambdatmp = 0.5*lambda;
}
DEBUG << "lambdatmp : " << lambdatmp;
lambda_prev = lambda;
fcp_prev = fcp;
if (not std::isfinite(lambdatmp))
{
lambda = get_program()->max_lambda(x, p);
xp = x + lambda*p;
retcode = 1;
break;
} else if ((lambdatmp < 0.1*lambda)) {
lambda = 0.1 * lambda;
} else {
lambda = lambdatmp;
}
DEBUG << "lambda end : " << lambda;
}
xp = x + lambda*p;
//sanitize(xp);
DEBUG << "xp : " << std::endl << xp;
base::compute_residuals(xp, new_res);
reformulate_residuals_cck_inplace(xp, new_res);
fcp = 0.5*new_res.squaredNorm();
++cnt;
} while(retcode == 2 and cnt < 100);
DEBUG << "Lambda : " << lambda;
if (cnt == 100)
{
ERROR << "Too much linesearch iterations ! We stop";
}
x = xp;
p = lambda*p;
return retcode;
}
} // end namespace micpsolver
} // end namespace specmicp
diff --git a/src/micpsolver/micpsolver_structs.hpp b/src/specmicp_common/micpsolver/micpsolver_structs.hpp
similarity index 100%
rename from src/micpsolver/micpsolver_structs.hpp
rename to src/specmicp_common/micpsolver/micpsolver_structs.hpp
diff --git a/src/micpsolver/micpsolverold.hpp b/src/specmicp_common/micpsolver/micpsolverold.hpp
similarity index 99%
rename from src/micpsolver/micpsolverold.hpp
rename to src/specmicp_common/micpsolver/micpsolverold.hpp
index 5348987..1ba4705 100644
--- a/src/micpsolver/micpsolverold.hpp
+++ b/src/specmicp_common/micpsolver/micpsolverold.hpp
@@ -1,326 +1,329 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#ifndef SPECMIC_MICPSOLVER_MICPSOLVEROLD_HPP
#define SPECMIC_MICPSOLVER_MICPSOLVEROLD_HPP
+#include "specmicp_common/types.hpp"
+
#include <memory>
#include <Eigen/Dense>
+
#include "micpsolver_structs.hpp"
#include "ncp_function.hpp"
//! \file micpsolver.hpp The MiCP solver
namespace specmicp {
namespace micpsolver {
//! \brief Call a NCP-function
//!
//! @tparam ncp_t the NCP function to use
//! @param a first argument
//! @param b second argument
//! @param t parameter of the NCP function
//!
template <NCPfunction ncp_t>
inline double ncp_function(double a, double b, double t);
//! \brief Reformulate the jacobian using the NCP-function ncp_t
//!
//! @tparam ncp_t the NCP-function to use
//! @param[in] neq number of equation
//! @param[in] neq_free number of free variables
//! @param[in] x the variables
//! @param[in] r the residuals
//! @param[in,out] jacobian the jacobian to reformulate
//! @param[in,out] r_reformulated the reformulated residuals
//! @param[in] t parameter of the NCP function
template <NCPfunction ncp_t>
inline void reformulate_jacobian_helper(
int neq,
int neq_free,
const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::MatrixXd& jacobian,
Eigen::VectorXd& r_reformulated,
double t
);
//! \brief reformulate the result of the Newton system
template <NCPfunction ncp_t>
inline void reformulate_result(
int neq,
int neq_free,
Eigen::VectorXd& x,
const Eigen::VectorXd& orig_r,
Eigen::VectorXd& grad_phi,
Eigen::VectorXd& update
);
//! \brief The MiCP Solver
//!
//! Solve
//! - \f$ G(u, v) = 0\f$
//! - \f$ 0 \leq v \perp H(u,v) \geq 0 \f$
//!
//! \tparam Program a subclass of MiCPProg
//!
//! References :
//! - \cite Munson2001
//! - \cite Facchinei2003
//!
template <class Program, NCPfunction ncp_t>
class MiCPSolverOLD {
public:
//! \brief Constructor
//!
//! @param prog smart pointer toward an instance of a Program to solve
MiCPSolverOLD(std::shared_ptr<Program> prog):
m_program(prog),
m_residuals(prog->total_variables()),
m_phi_residuals(Eigen::VectorXd::Zero(prog->total_variables())),
m_jacobian(prog->total_variables(),prog ->total_variables()),
m_max_taken(false),
m_consec_max(0)
{
}
//! \brief Return a const reference to the options used by the algorithm
const MiCPSolverOptions& get_options() const {return m_options;}
//! \brief Return a reference to the options used by the algorithm
MiCPSolverOptions& get_options() {return m_options;}
//! \brief Set the options
void set_options(const MiCPSolverOptions& options) {m_options = options;}
//! \brief Return the number of equations
int get_neq() const {return m_program->total_variables();}
//! \brief Return the number of equations corresponding to the free variables (size of G)
int get_neq_free() const {return m_program->nb_free_variables();}
// Merit function
// ##############
//! \brief Reformulation for lower bounded variable
double phi_lower_bounded(const double& x, const double& r, const double& l) const {
return ncp_function<ncp_t>(x-l, r, get_options().penalization_factor);}
//! \brief Reformulation for a free variable
double phi_free(const double& r) const {
return r;
}
//! \brief Compute the norm of the update
template <int p>
double norm_update(const Eigen::VectorXd& update,
const Eigen::VectorXd& solution) const {
return (update.array().abs()/(solution.array().max(1))
).matrix().template lpNorm<p>();
}
// Residuals and jacobian
// ----------------------
//! \brief Compute the residuals, store it in r
//!
//! @param[in] x the variables
//! @param[out] r vector to store the residuals (must be of the same size as x)
void compute_residuals(const Eigen::VectorXd& x, Eigen::VectorXd& r)
{
m_program->get_residuals(x, r);
get_perf().nb_call_residuals += 1;
}
//! \brief Compute the residuals, use internal storage
//!
//! @param[in] x the variables
void compute_residuals(const Eigen::VectorXd& x)
{
return compute_residuals(x, m_residuals);
get_perf().nb_call_jacobian += 1;
}
//! \brief Compute the jacobian
//!
//! Assumes that the residual have been computed before
//!
//! @param[in] x the variables
void compute_jacobian(Eigen::VectorXd& x)
{
m_program->get_jacobian(x, m_jacobian);
}
//! \brief Reformulation of the residuals
//!
//! Reformulate the problem - assumes that r, the residual, has been computed before
//!
//! @param[in] x the variables
//! @param[in] r the residuals
//! @param[out] r_phi a vector of size neq, which will contain the reformulated residuals
void reformulate_residuals(const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::VectorXd& r_phi);
//! \brief Reformulation of the residuals *inplace*
//!
//! @param[in] x the variables
//! @param[in,out] r the residual, will contain the reformulated residuals
void reformulate_residuals_inplace(const Eigen::VectorXd& x,
Eigen::VectorXd &r);
//! \brief Reformulation of the jacobian
//!
//! r is the original vector of residuals (not reformulated)
void reformulate_jacobian(const Eigen::VectorXd& x
)
{
reformulate_jacobian_helper<ncp_t>(get_neq(), get_neq_free(),
x, m_residuals,
m_jacobian,
m_phi_residuals,
get_options().penalization_factor);
}
//! \brief Compute the factors to scale the jacobian
//!
//! @param[in] jacobian the jacobian to scale (from the reformulated problem)
//! @param[in] residual the residuals corresponding to the jacobian
//! @param[out] rscaler scaling factors of the rows
//! @param[out] cscaler scaling factors of the columns
void scaling_jacobian(const Eigen::MatrixXd& jacobian,
const Eigen::VectorXd& residual,
Eigen::VectorXd& rscaler,
Eigen::VectorXd& cscaler);
// Algorithm
// #########
//! \brief Solver the program using x as initial guess
//!
//! @param[in,out] x the initial guess, as output contains the solution (from the last iteration)
MiCPSolverReturnCode solve(Eigen::VectorXd& x);
//! \brief Setup the residuals
//!
//! @param[in] x the current solution
void setup_residuals(const Eigen::VectorXd& x) {
compute_residuals(x);
reformulate_residuals(x, m_residuals, m_phi_residuals);
}
//! \brief Setup the jacobian
//!
//! @param[in] x the current solution
void setup_jacobian(Eigen::VectorXd& x) {
compute_jacobian(x);
reformulate_jacobian(x);
m_grad_phi = m_jacobian.transpose() * m_phi_residuals;
}
//! \brief Check for convergence
//!
//! @param nb_iterations the number of iterations
//! @param update the update taken at the previous iteration
//! @param solution the current solution
//! @return a MiCPSolverReturnCode describing the state of the algorithm
MiCPSolverReturnCode check_convergence(int nb_iterations,
Eigen::VectorXd& update,
Eigen::VectorXd& solution);
//! \brief Solve the Newton system
//!
//! Assume that the Newton system has been formed
//!
//! \param[out] update the update to apply to the solution
MiCPSolverReturnCode search_direction_calculation(Eigen::VectorXd& update);
//! \brief Solve the Newton system - does not scale the jacobian
//!
//! Assume that the Newton system has been formed
//!
//! \param[out] update the update to apply to the solution
MiCPSolverReturnCode search_direction_calculation_no_scaling(Eigen::VectorXd& update);
//! \brief Linesearch
//!
//! Nonmonotone linesearch
//!
//! References :
//! - \cite Dennis1983
//! - \cite Munson2001
//!
int linesearch(Eigen::VectorXd& update, Eigen::VectorXd& x);
//! \brief Crashing
//!
//! This function improves, if possible, the initial guess
//! Reference :
//! - \cite Munson2001
void crashing(Eigen::VectorXd& x);
//! \brief Project variables on the feasible set
void projection(Eigen::VectorXd& x);
//! \brief Return a const reference to an instance of MiCPPerformance
const MiCPPerformance& get_performance() {return m_perf;}
protected:
//! \brief Return a reference to an instance of MiCPPerformance
MiCPPerformance& get_perf() {return m_perf;}
private:
//! \brief Return the step corrected step length if it is too long
double is_step_too_long(Eigen::VectorXd& update);
std::shared_ptr<Program> m_program; //!< Smart pointer of a program
MiCPSolverOptions m_options; //!< The options
MiCPPerformance m_perf; //!< The performance
// Residuals and jacobian
Eigen::VectorXd m_residuals; //!< The residuals
Eigen::VectorXd m_phi_residuals; //!< The reformulated residuals
Eigen::VectorXd m_grad_phi; //!< The gradient of the reformulated residuals
Eigen::MatrixXd m_jacobian; //!< The jacobian
std::vector<double> m_max_merits; //!< Contains the m best value of the merit function
bool m_max_taken; //!< True if the 'length' of the step was equal or bigger than the maximum step length
int m_consec_max; //!< The number of consecutive step were the step length was equal or bigger than the maximum step length
bool m_gradient_step_taken; //!< True if the update was computed using the gradient
};
} // end namespace micpsolver
} // end namespace specmicp
// ###############//
// Implementation //
// ###############//
#include "micpsolverold.inl"
#endif // SPECMIC_MICPSOLVER_MICPSOLVER_HPP
diff --git a/src/micpsolver/micpsolverold.inl b/src/specmicp_common/micpsolver/micpsolverold.inl
similarity index 99%
rename from src/micpsolver/micpsolverold.inl
rename to src/specmicp_common/micpsolver/micpsolverold.inl
index d3d9ec7..0962740 100644
--- a/src/micpsolver/micpsolverold.inl
+++ b/src/specmicp_common/micpsolver/micpsolverold.inl
@@ -1,639 +1,637 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#include "micpsolverold.hpp" // for syntaxic coloration...
#include "estimate_cond_number.hpp"
-#include "utils/log.hpp"
-
-#include <iostream>
+#include "specmicp_common/log.hpp"
//! \file micpsolver.inl implementation of the MiCP solver
namespace specmicp {
namespace micpsolver {
// Main algorithm
// ##############
template <class Program, NCPfunction ncp_t>
MiCPSolverReturnCode MiCPSolverOLD<Program, ncp_t>::solve(Eigen::VectorXd &x)
{
int cnt = 0;
if (get_options().use_crashing) crashing(x);
else setup_residuals(x);
MiCPSolverReturnCode retcode = MiCPSolverReturnCode::NotConvergedYet;
Eigen::VectorXd update(get_neq());
while (retcode == MiCPSolverReturnCode::NotConvergedYet)
{
DEBUG << "Iteration : " << cnt;
SPAM << "Solution : \n" << x;
m_program->hook_start_iteration(x, m_phi_residuals.norm());
setup_residuals(x);
get_perf().current_residual = m_phi_residuals.norm();
SPAM << "Residuals : \n ----- \n" << m_phi_residuals << "\n ----- \n";
retcode = check_convergence(cnt, update, x);
get_perf().return_code = retcode;
if (retcode != MiCPSolverReturnCode::NotConvergedYet) break;
++cnt;
m_max_taken = false;
setup_jacobian(x);
if(get_options().use_scaling)
search_direction_calculation(update);
else
search_direction_calculation_no_scaling(update);
reformulate_result<ncp_t>(get_neq(), get_neq_free(),
x, m_residuals,
m_grad_phi, update);
int termcode = linesearch(update, x);
get_perf().current_update = update.norm();
DEBUG << "Return LineSearch : " << termcode;
projection(x);
get_perf().nb_iterations = cnt;
}
return retcode;
}
template <class Program, NCPfunction ncp_t>
MiCPSolverReturnCode MiCPSolverOLD<Program, ncp_t>::check_convergence(int nb_iterations,
Eigen::VectorXd& update,
Eigen::VectorXd& solution)
{
MiCPSolverReturnCode termcode = MiCPSolverReturnCode::NotConvergedYet;
const double norm_residuals = m_phi_residuals.lpNorm<Eigen::Infinity>();
if (norm_residuals < get_options().fvectol)
{
termcode = MiCPSolverReturnCode::ResidualMinimized;
}
else if (nb_iterations >0 and norm_update<Eigen::Infinity>(update, solution) < get_options().steptol)
{
if (norm_residuals > get_options().threshold_stationary_point)
{
ERROR << "Stationary point detected !";
termcode = MiCPSolverReturnCode::StationaryPoint;
}
WARNING << "Error is minimized - may indicate a stationnary point";
termcode = MiCPSolverReturnCode::ErrorMinimized;
}
else if (nb_iterations > get_options().max_iter)
{
ERROR << "Maximum number of iteration reached (" << get_options().max_iter << ")";
termcode = MiCPSolverReturnCode::MaxIterations;
}
else if (m_max_taken)
{
++m_consec_max;
if (m_consec_max == get_options().maxiter_maxstep) {
ERROR << "Divergence detected - Maximum step length taken two many times";
termcode = MiCPSolverReturnCode::MaxStepTakenTooManyTimes;
}
}
else
{
m_consec_max = 0;
}
return termcode;
}
template <class Program, NCPfunction ncp_t>
MiCPSolverReturnCode MiCPSolverOLD<Program, ncp_t>::search_direction_calculation(Eigen::VectorXd& update)
{
Eigen::VectorXd rscaler(Eigen::VectorXd::Ones(m_jacobian.cols()));
Eigen::VectorXd cscaler(Eigen::VectorXd::Ones(m_jacobian.rows()));
scaling_jacobian(m_jacobian, m_phi_residuals, rscaler, cscaler);
m_jacobian = rscaler.asDiagonal() * (m_jacobian) * cscaler.asDiagonal();
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver;
m_gradient_step_taken = false;
int m;
for (m=0; m<get_options().max_factorization_step; ++m)
{
const double lambda = get_options().factor_gradient_search_direction;
solver.compute(m_jacobian);
get_perf().nb_factorization += 1;
if (solver.info() != Eigen::Success or not solver.isInvertible())
{
DEBUG << "Solver.info : " << solver.info() << " - is invertible : " << solver.isInvertible();
ERROR << "System cannot be solved, we try a perturbation";
m_gradient_step_taken = true;
m_jacobian += rscaler.asDiagonal() * (
lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols())) * cscaler.asDiagonal();
continue;
}
double cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
if (cond > get_options().condition_limit)
{
m_gradient_step_taken = true;
m_jacobian += rscaler.asDiagonal() * (
lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols())) * cscaler.asDiagonal();
continue;
}
update = solver.solve(-rscaler.cwiseProduct(m_phi_residuals + m*lambda*m_grad_phi));
update = cscaler.cwiseProduct(update);
double descent_cond = m_grad_phi.dot(update);
double norm_grad = m_grad_phi.norm();
double norm_update = update.norm();
if ( (descent_cond <= -get_options().factor_descent_condition*std::min(std::pow(norm_update,2),std::pow(norm_update,3)))
and (descent_cond <= -get_options().factor_descent_condition*std::min(std::pow(norm_grad,2),std::pow(norm_grad,3)))
)
break; // we have a solution !
m_gradient_step_taken = true;
m_jacobian += rscaler.asDiagonal() * ( lambda*
Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols())
) * cscaler.asDiagonal();
}
DEBUG << "Gradient step : m = " << m;
if (m == get_options().max_factorization_step) {
INFO << "Full gradient step taken !";
update = -m_grad_phi;
}
return MiCPSolverReturnCode::NotConvergedYet;
}
template <class Program, NCPfunction ncp_t>
MiCPSolverReturnCode MiCPSolverOLD<Program, ncp_t>::search_direction_calculation_no_scaling(Eigen::VectorXd& update)
{
DEBUG << "Solving linear system";
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver;
m_gradient_step_taken = false;
int m;
for (m=0; m<get_options().max_factorization_step; ++m)
{
const double lambda = get_options().factor_gradient_search_direction;
solver.compute(m_jacobian);
get_perf().nb_factorization += 1;
if (solver.info() != Eigen::Success or not solver.isInvertible()) continue;
double cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
if (cond > get_options().condition_limit)
{
continue;
}
update = solver.solve(-(m_phi_residuals + m*lambda*m_grad_phi));
double descent_cond = m_grad_phi.dot(update);
double norm_grad = m_grad_phi.norm();
double norm_update = update.norm();
if ( (descent_cond <= -get_options().factor_descent_condition*std::min(std::pow(norm_update,2),std::pow(norm_update,3)))
and (descent_cond <= -get_options().factor_descent_condition*std::min(std::pow(norm_grad,2),std::pow(norm_grad,3)))
)
break; // we have a solution !
m_gradient_step_taken = true;
m_jacobian += lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols());
}
DEBUG << "Gradient step : m = " << m;
if (m ==4) {
INFO << "Full gradient step taken !";
update = -m_grad_phi;
}
return MiCPSolverReturnCode::NotConvergedYet;
}
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>::crashing(Eigen::VectorXd &x)
{
DEBUG << "Crashing ";
const double beta = 0.5;
const double sigma = 1e-5;
int cnt = 0;
while (cnt < 10)
{
setup_residuals(x);
setup_jacobian(x);
m_grad_phi = m_jacobian.transpose()*m_phi_residuals;
Eigen::VectorXd xp(get_neq());
int l=0;
const int maxl = 10;
while (l<maxl)
{
xp = x - std::pow(beta, l)*m_grad_phi;
for (int i=get_neq_free(); i<get_neq(); ++i)
{
if (xp(i) < 0) xp(i) = 0;
}
Eigen::VectorXd new_res(get_neq());
compute_residuals(x, new_res);
reformulate_residuals_inplace(x, new_res);
double test = 0.5*(new_res.squaredNorm() - m_phi_residuals.squaredNorm()) + sigma*m_grad_phi.dot(x - xp);
if (test <= 0) break;
++l;
}
if (l == maxl) break;
x = xp;
++cnt;
}
get_perf().nb_crashing_iterations = cnt;
DEBUG << "Crashing iterations : " << cnt;
m_max_merits.reserve(4);
SPAM << "Solution after crashing \n ------ \n " << x << "\n ----- \n";
}
// Others
// ######
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>::reformulate_residuals(const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::VectorXd& r_phi)
{
r_phi.resizeLike(r);
r_phi.block(0, 0, get_neq_free(), 1) = r.block(0, 0, get_neq_free(), 1);
for (int i = get_neq_free(); i<get_neq(); ++i)
{
r_phi(i) = phi_lower_bounded(x(i), r(i), 0);
}
}
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>::reformulate_residuals_inplace(const Eigen::VectorXd& x,
Eigen::VectorXd& r)
{
for (int i = get_neq_free(); i<get_neq(); ++i)
{
r(i) = phi_lower_bounded(x(i), r(i), 0);
}
}
// ref : Munson et al. (2001)
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>:: scaling_jacobian(
const Eigen::MatrixXd& jacobian,
const Eigen::VectorXd& r_phi,
Eigen::VectorXd& rscaler,
Eigen::VectorXd& cscaler)
{
for (int i=0; i<jacobian.cols(); ++i)
{
const double sumhsq = jacobian.row(i).array().square().sum();
double s = std::sqrt(r_phi(i)*r_phi(i) + sumhsq);
rscaler(i) = 1.0/std::max(s, 1e-10);
}
for (int i=0; i<jacobian.cols(); ++i)
{
const double sumhsq = (rscaler.asDiagonal()*jacobian).col(i).array().square().sum();
double s = std::sqrt(sumhsq);
cscaler(i) = 1.0/std::max(s, 1e-10);
}
}
template <class Program, NCPfunction ncp_t>
int MiCPSolverOLD<Program, ncp_t>::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x)
{
// Reference Algo A6.3.1 : Dennis and Schnabel (1983)
DEBUG << "Linesearch";
Eigen::VectorXd xp(get_neq());
Eigen::VectorXd new_res(get_neq());
double fcp;
m_max_taken = false;
int retcode = 2;
const double alpha = 1e-6;
double newtlen = is_step_too_long(p);
double init_slope = m_grad_phi.dot(p);
double rellength = std::abs(p(0));
for (int i=1; i<get_neq(); ++i)
{
rellength = std::max(rellength, std::abs(p(i)));
}
double minlambda = get_options().steptol / rellength;
double lambda = m_program->max_lambda(x, p);
double lambda_prev = lambda;
// non monotone linesearch
// -----------------------
double merit_value = 0.5*m_phi_residuals.squaredNorm();
// new residual
xp = x + lambda*p;
compute_residuals(xp, new_res);
reformulate_residuals_inplace(xp, new_res);
fcp = 0.5*new_res.squaredNorm();
// Skip linesearch if enough progress is done
if (fcp < get_options().coeff_accept_newton_step *merit_value)
{
if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value;
else m_max_merits.push_back(merit_value);
x = xp;
return 0;
}
//std::cout << "Merit value : " << merit_value << std::endl;
double mmax = merit_value;
if (m_max_merits.size() > 0)
{
mmax = m_max_merits[m_max_merits.size()-1];
}
if (m_max_merits.size() < 4)
{
m_max_merits.push_back(merit_value);
if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4;
}
else if (merit_value < mmax)
{
m_max_merits[3] = merit_value;
merit_value = mmax;
}
if (m_gradient_step_taken)
{
merit_value *= 100;
}
//std::cout << "Merit value used : " << merit_value << std::endl;
double fc = merit_value;
double fcp_prev;
int cnt = 0;
do
{
fcp = 0.5*new_res.squaredNorm();
//std::cout << "fcp : " << fcp << "\n fc+alin : " << fc+alpha*lambda*init_slope << " # fc : " << fc << std::endl;
if (fcp <= fc - std::min(-alpha*lambda*init_slope,(1-alpha)*fc)) //pg760 Fachinei2003
{
retcode = 0;
if (lambda ==1 and (newtlen > 0.99 * get_options().maxstep)) {
m_max_taken = true;
}
break;
}
else if (lambda < minlambda)
{
retcode = 1;
break;
}
else
{
double lambdatmp;
if (cnt == 0) { // only a quadratic at the first
lambdatmp = - init_slope / (2*(fcp - fc -init_slope));
}
else
{
const double factor = 1 /(lambda - lambda_prev);
const double x1 = fcp - fc - lambda*init_slope;
const double x2 = fcp_prev - fc - lambda_prev*init_slope;
const double a = factor * ( x1/(lambda*lambda) - x2/(lambda_prev*lambda_prev));
const double b = factor * ( -x1*lambda_prev/(lambda*lambda) + x2*lambda/(lambda_prev*lambda_prev));
if (a == 0)
{ // cubic interpolation is in fact a quadratic interpolation
lambdatmp = - init_slope/(2*b);
}
else
{
const double disc = b*b-3*a*init_slope;
lambdatmp = (-b+std::sqrt(disc))/(3*a);
}
if (lambdatmp > 0.5*lambda ) lambdatmp = 0.5*lambda;
}
lambda_prev = lambda;
fcp_prev = fcp;
if (lambdatmp < 0.1*lambda) {
lambda = 0.1 * lambda;
} else {
lambda = lambdatmp;
}
}
xp = x + lambda*p;
compute_residuals(xp, new_res);
reformulate_residuals_inplace(xp, new_res);
++cnt;
} while(retcode == 2 and cnt < 100);
DEBUG << "Lambda : " << lambda;
if (cnt == 100)
{
ERROR << "Too much linesearch iterations ! We stop";
}
x = xp;
p = lambda*p;
return retcode;
}
// Projection of the variables onto the feasible set
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>::projection(Eigen::VectorXd &x)
{
for (int i=0; i<m_program->nb_complementarity_variables(); ++i)
{
if (x(i+m_program->nb_free_variables()) < get_options().projection_min_variable)
{
x(i+m_program->nb_free_variables()) = 0;
}
}
}
template <class Program, NCPfunction ncp_t>
double MiCPSolverOLD<Program, ncp_t>::is_step_too_long(Eigen::VectorXd& update)
{
double steplength = update.norm();
if (steplength > get_options().maxstep)
{
m_max_taken = true;
update = get_options().maxstep / steplength * update;
steplength = get_options().maxstep;
}
return steplength;
}
// ================================================= //
// //
// NCP functions and reformulation //
// //
// ================================================= //
template <>
inline double ncp_function<NCPfunction::penalizedFB>(double a, double b, double t)
{
return penalized_fisher_burmeister(a, b, t);
}
template <>
inline double ncp_function<NCPfunction::min>(double a, double b, double _)
{
return std::min(a, b);
}
template <>
inline void reformulate_jacobian_helper<NCPfunction::penalizedFB>(
int neq,
int neq_free,
const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::MatrixXd& jacobian,
Eigen::VectorXd& _,
double t
)
{
// set the z vector : contains 1 for degenerate points
Eigen::VectorXd z(Eigen::VectorXd::Zero(neq));
for (int i=neq_free; i<neq; ++i)
{
if (x(i) == 0 and r(i) == 0)
z(i) = 1.0;
}
// modify the jacobian
const double lambda = t;
for (int i=neq_free; i<neq; ++i)
{
Eigen::VectorXd grad_fi = jacobian.block(i, 0, 1, neq).transpose();
if (z(i) != 0)
{
double gpdotz = grad_fi.dot(z);
double s = std::abs(z(i)) + std::abs(gpdotz);
s = s * std::sqrt(z(i)*z(i)/(s*s) + gpdotz*gpdotz/(s*s));
const double c = lambda*(z(i)/s - 1);
const double d = lambda*(gpdotz/s -1);
grad_fi = d*grad_fi;
grad_fi(i) += c;
}
else
{
double s = std::abs(x(i)) + std::abs(r(i));
s = s * std::sqrt(x(i)*x(i)/(s*s) + r(i)*r(i)/(s*s));
double c = lambda*(x(i)/s - 1);
double d = lambda*(r(i)/s - 1);
if ((lambda <1) and (r(i) > 0) and (x(i) >0))
{
c -= (1-lambda)*r(i);
d -= (1-lambda)*x(i);
}
grad_fi = d*grad_fi;
grad_fi(i) += c;
}
jacobian.block(i, 0, 1, neq) = grad_fi.transpose();
}
}
template <>
inline void reformulate_jacobian_helper<NCPfunction::min>(
int neq,
int neq_free,
const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::MatrixXd& jacobian,
Eigen::VectorXd& r_phi,
double _
)
{
std::vector<int> to_keep;
to_keep.reserve(10);
Eigen::VectorXd to_remove(neq-neq_free);
for (int i=neq_free; i<neq; ++i)
{
if (x(i) >= r(i))
{
to_remove(i-neq_free) = 0;
to_keep.push_back(i);
}
else
to_remove(i-neq_free) = x(i);
}
r_phi.block(0, 0, neq_free, 1) -= jacobian.block(0, neq_free, neq_free, neq-neq_free)*to_remove;
int new_i = neq_free;
for (auto it=to_keep.begin(); it!=to_keep.end(); ++it)
{
//r_phi.block(0, 0, neq_free, 1) += x(*it)*jacobian.block(0, *it, neq_free, 1);
jacobian.block(new_i, 0, 1, neq_free) = jacobian.block(*it, 0, 1, neq_free); // the bottom right corner is 0
jacobian.block(0, new_i, neq_free, 1) = jacobian.block(0, *it, neq_free, 1);
r_phi(new_i) = r_phi(*it);
++new_i;
}
r_phi.conservativeResize(new_i);
jacobian.conservativeResize(new_i, new_i);
DEBUG << jacobian;
}
template <>
inline void reformulate_result<NCPfunction::penalizedFB>(
int neq,
int neq_free,
Eigen::VectorXd& x,
const Eigen::VectorXd& orig_r,
Eigen::VectorXd& grad_phi,
Eigen::VectorXd& update
)
{}
template <>
inline void reformulate_result<NCPfunction::min>(
int neq,
int neq_free,
Eigen::VectorXd& x,
const Eigen::VectorXd& orig_r,
Eigen::VectorXd& grad_phi,
Eigen::VectorXd& update
)
{
//std::cout << " Update \n ------- \n " << update << std::endl;
int tot_to_keep = 0;
for (int i=neq_free; i<neq; ++i)
{
if (x(i) >= orig_r(i))
++tot_to_keep;
}
//std::cout << " update \n ------ \n" << update.block(neq_free, 0, tot_to_keep, 1) << std::endl;
update.conservativeResize(neq);
grad_phi.conservativeResize(neq);
int kept_i = 1;
for (int i=neq-1; i>=neq_free; --i)
{ // we go backwards to avoid extra copies
//std::cout << i << " # " << x(i) << " - " << orig_r(i) << std::endl;
if (x(i) >= orig_r(i))
{
//std::cout << i << std::endl;
update(i) = update(neq_free+(tot_to_keep-kept_i));
//std::cout << update(i) << std::endl;
grad_phi(i) = grad_phi(neq_free+(tot_to_keep-kept_i));
++kept_i;
}
else
{
//x(i) = 0.0;
//update(i) = 0.0;
update(i) = -x(i);
grad_phi(i) = x(i);
}
}
}
} // end namespace micpsolver
} // end namespace specmicp
diff --git a/src/micpsolver/ncp_function.hpp b/src/specmicp_common/micpsolver/ncp_function.hpp
similarity index 98%
rename from src/micpsolver/ncp_function.hpp
rename to src/specmicp_common/micpsolver/ncp_function.hpp
index 3042ae4..29eb6eb 100644
--- a/src/micpsolver/ncp_function.hpp
+++ b/src/specmicp_common/micpsolver/ncp_function.hpp
@@ -1,94 +1,94 @@
/* =============================================================================
Copyright (c) 2014 - 2016
F. Georget <fabieng@princeton.edu> Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
============================================================================= */
#ifndef SPECMICP_MICPSOLVE_NCPFUNCTION_HPP
#define SPECMICP_MICPSOLVE_NCPFUNCTION_HPP
#include <cmath>
-#include "../macros.hpp"
+#include "specmicp_common/macros.hpp"
//! \file ncp_function.hpp implements the ncp function
namespace specmicp {
namespace micpsolver {
//! \brief The Fisher-Burmeister NCP-function
//!
//! \param a first variable of the NCP-function
//! \param b second variable of the NCP-function
//!
//! References:
//! - \cite Munson2001
//! - \cite Facchinei2003
template <typename ScalarT>
ScalarT SPECMICP_CONST_F fisher_burmeister(ScalarT a, ScalarT b)
{
ScalarT s = std::abs(a) + std::abs(b);
if (s != 0)
{
s = s*std::sqrt((a*a)/(s*s) + (b*b)/(s*s));
}
if ( a + b <= 0)
{
return s - (a + b);
}
else
{
return -2*a*b/(s + (a+b));
}
}
//! \brief The penalized Fisher-Burmeister NCP-function
//!
//! \param t in (0, 1), penalization factor
//! \param a first variable of the NCP-function
//! \param b second varaible of the NCP-function
//!
//! References:
//! - \cite Chen1997a
//! - \cite Chen2000
//! - \cite Munson2001
template <typename ScalarT>
ScalarT SPECMICP_CONST_F penalized_fisher_burmeister(ScalarT a, ScalarT b, ScalarT t)
{
specmicp_assert(t >= 0);
specmicp_assert(t <= 1);
return t*fisher_burmeister(a,b)-(1.0-t)*std::max(0.0, a)*std::max(0.0, b);
}
} // end namespace micpsolver
} // end namespace specmicp
#endif // SPECMICP_MICPSOLVE_NCPFUNCTION_HPP
diff --git a/tests/micpsolver/ncp_functions.cpp b/tests/micpsolver/ncp_functions.cpp
deleted file mode 100644
index 3cd9b70..0000000
--- a/tests/micpsolver/ncp_functions.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-#include "catch.hpp"
-
-#include "micpsolver/ncp_function.hpp"
-
-using specmicp::micpsolver::fisher_burmeister;
-using specmicp::micpsolver::penalized_fisher_burmeister;
-
-TEST_CASE("NCP functions") {
- SECTION("fisher Burmeister") {
- CHECK(fisher_burmeister(0.0, 0.0) == 0.0);
- CHECK(fisher_burmeister(0.0, 1.0) == 0.0);
- CHECK(fisher_burmeister(0.0, 3.4) == 0.0);
- CHECK(std::abs(fisher_burmeister(1.0, 1.0)-std::sqrt(2)+2) == Approx(0.0));
- CHECK(std::abs(fisher_burmeister(2.0, 1.0)-std::sqrt(5)+3) == Approx(0.0));
- }
-
- SECTION("Penalized Fischer Burmeister") {
- CHECK(penalized_fisher_burmeister(0.0, 0.0, 0.5) == 0.0);
- CHECK(penalized_fisher_burmeister(0.0, 1.0, 0.3) == 0.0);
- CHECK(penalized_fisher_burmeister(0.0, 3.4, 0.5) == 0.0);
- CHECK(penalized_fisher_burmeister(8.4, 0.0, 0.2) == 0.0);
- CHECK(std::abs(fisher_burmeister(1.0, 1.0)-penalized_fisher_burmeister(1.0, 1.0, 1.0)) == Approx(0.0));
- CHECK(std::abs(penalized_fisher_burmeister(2.0, 1.0, 0.5)-0.5*(std::sqrt(5)-3)+0.5*2.0) == Approx(0.0));
- }
-}
diff --git a/tests/micpsolver/test_micpsolver.cpp b/tests/micpsolver/test_micpsolver.cpp
deleted file mode 100644
index 178916e..0000000
--- a/tests/micpsolver/test_micpsolver.cpp
+++ /dev/null
@@ -1,3 +0,0 @@
-#define CATCH_CONFIG_MAIN
-#include "catch.hpp"
-
diff --git a/tests/specmicp_common/CMakeLists.txt b/tests/specmicp_common/CMakeLists.txt
new file mode 100644
index 0000000..a02145a
--- /dev/null
+++ b/tests/specmicp_common/CMakeLists.txt
@@ -0,0 +1,63 @@
+# Catch test
+#===========
+
+#common
+
+# mock test library
+add_library(test_to_load SHARED EXCLUDE_FROM_ALL
+ test_to_load.cpp
+)
+set_target_properties(test_to_load PROPERTIES PREFIX "")
+
+
+add_custom_target(test_pasic_plugin_incl
+ SOURCES test_basic_plugin_mockobject.hpp
+)
+
+add_library(test_basic_plugin SHARED EXCLUDE_FROM_ALL
+ test_basic_plugin.cpp
+)
+set_target_properties(test_basic_plugin PROPERTIES PREFIX "")
+target_link_libraries(test_basic_plugin specmicp_common_static ${YAML_LIBRARIES})
+
+
+set(SPECMICP_COMMON_TEST_FILES
+ test_common.cpp
+
+ butchertableau.cpp
+ cli.cpp
+ condition_number.cpp
+ dynamic_loader.cpp
+ embeddedrungekutta.cpp
+ laws.cpp
+ micpsolver.cpp
+ misc.cpp
+ plugin_manager.cpp
+ range_iterator.cpp
+ units.cpp
+ value_checker.cpp
+ vector_checker.cpp
+ sparse_solvers.cpp
+ yaml.cpp
+)
+
+set_source_files_properties(
+ misc.cpp
+ PROPERTIES
+ COMPILE_DEFINITIONS "CURRENT_DIRECTORY=\"${CMAKE_CURRENT_BINARY_DIR}\""
+ )
+
+if (HDF5_FOUND)
+ list(APPEND SPECMICP_COMMON_TEST_FILES
+ hdf5_all.cpp
+ )
+ include_directories(${HDF5_INCLUDE_DIRS})
+ set_source_files_properties(${COMMON_TEST_DIR}/hdf5_all.cpp
+ PROPERTIES COMPILE_DEFINITIONS HDF5_DEFINITIONS)
+endif()
+
+add_catch_test(NAME common SOURCES
+ ${SPECMICP_COMMON_TEST_FILES}
+ LINK_LIBRARIES specmicp_common_static ${YAML_LIBRARIES}
+ DEPENDS test_to_load test_basic_plugin
+)
diff --git a/tests/common/butchertableau.cpp b/tests/specmicp_common/butchertableau.cpp
similarity index 100%
rename from tests/common/butchertableau.cpp
rename to tests/specmicp_common/butchertableau.cpp
diff --git a/tests/common/cli.cpp b/tests/specmicp_common/cli.cpp
similarity index 100%
rename from tests/common/cli.cpp
rename to tests/specmicp_common/cli.cpp
diff --git a/tests/micpsolver/condition_number.cpp b/tests/specmicp_common/condition_number.cpp
similarity index 97%
rename from tests/micpsolver/condition_number.cpp
rename to tests/specmicp_common/condition_number.cpp
index 3acff2e..24760c0 100644
--- a/tests/micpsolver/condition_number.cpp
+++ b/tests/specmicp_common/condition_number.cpp
@@ -1,94 +1,94 @@
#include "catch.hpp"
+#include "specmicp_common/micpsolver/estimate_cond_number.hpp"
#include <Eigen/Dense>
-#include "micpsolver/estimate_cond_number.hpp"
TEST_CASE("Condition number") {
SECTION("Lower matrix") {
Eigen::MatrixXd mat(4,4);
mat << 1, 0, 0 , 0,
1, 1, 0, 0,
1, 1, 1, 0,
1, 1, 1, 1;
double cond = specmicp::micpsolver::estimate_condition_number(
mat.triangularView<Eigen::Lower>());
CHECK(cond == 4);
}
SECTION("Upper matrix") {
Eigen::MatrixXd mat(4,4);
mat << 1, 0, 0 , 0,
1, 1, 0, 0,
1, 1, 1, 0,
1, 1, 1, 1;
double cond = specmicp::micpsolver::estimate_condition_number(
mat.transpose().triangularView<Eigen::Upper>());
CHECK(cond == 4);
}
SECTION("Lower matrix, 1000") {
Eigen::MatrixXd mat(4, 4);
mat << 1, 0, 0 , 0,
1, 1, 0, 0,
1, 1, 1, 0,
1000, 1, 1, 1;
double cond = specmicp::micpsolver::estimate_condition_number(
mat.triangularView<Eigen::Lower>());
CHECK(cond == 1003);
}
SECTION("Lower matrix 1000 2") {
Eigen::MatrixXd mat(4, 4);
mat << 1, 0, 0 , 0,
1, 2000, 0, 0,
1, 1, 1, 0,
1000, 1, 1, 1000;
double cond = specmicp::micpsolver::estimate_condition_number(
mat.triangularView<Eigen::Lower>());
CHECK(cond == 2002);
}
SECTION("Lower matrix 10000 ") {
Eigen::MatrixXd mat(4, 4);
mat << 1, 0, 0 , 0,
1, 1000, 0, 0,
1, 1, 1, 0,
1000, 1, 1, 10000;
double cond = specmicp::micpsolver::estimate_condition_number(
mat.triangularView<Eigen::Lower>());
CHECK(cond == 11002);
}
SECTION("Lower matrix signs ") {
Eigen::MatrixXd mat(4, 4);
mat << -1, 0, 0 , 0,
1, -2000, 0, 0,
-8, 1, -5, 0,
1000, 1, 1, -10000;
double cond = specmicp::micpsolver::estimate_condition_number(
mat.triangularView<Eigen::Lower>());
CHECK(cond == 11002);
}
SECTION("Random") {
for (int i=0; i<20; ++i)
{
Eigen::MatrixXd mat(20, 20);
mat.setRandom();
mat.triangularView<Eigen::Upper>().setZero();
double norm = mat.norm();
double norm_inv = mat.inverse().norm();
double cond = specmicp::micpsolver::estimate_condition_number(
mat.triangularView<Eigen::Lower>());
CHECK(abs(cond - (norm_inv/norm)) < 1.0);
}
}
}
diff --git a/tests/common/dynamic_loader.cpp b/tests/specmicp_common/dynamic_loader.cpp
similarity index 100%
rename from tests/common/dynamic_loader.cpp
rename to tests/specmicp_common/dynamic_loader.cpp
diff --git a/tests/common/embeddedrungekutta.cpp b/tests/specmicp_common/embeddedrungekutta.cpp
similarity index 100%
rename from tests/common/embeddedrungekutta.cpp
rename to tests/specmicp_common/embeddedrungekutta.cpp
diff --git a/tests/common/hdf5_all.cpp b/tests/specmicp_common/hdf5_all.cpp
similarity index 100%
rename from tests/common/hdf5_all.cpp
rename to tests/specmicp_common/hdf5_all.cpp
diff --git a/tests/common/laws.cpp b/tests/specmicp_common/laws.cpp
similarity index 100%
rename from tests/common/laws.cpp
rename to tests/specmicp_common/laws.cpp
diff --git a/tests/micpsolver/micpsolver.cpp b/tests/specmicp_common/micpsolver.cpp
similarity index 98%
copy from tests/micpsolver/micpsolver.cpp
copy to tests/specmicp_common/micpsolver.cpp
index 80e5035..9253774 100644
--- a/tests/micpsolver/micpsolver.cpp
+++ b/tests/specmicp_common/micpsolver.cpp
@@ -1,241 +1,241 @@
/*-------------------------------------------------------
- Module : tests/micpsolver
- File : test_micpsolver.hpp
- Author : Fabien Georget
Copyright (c) 2014, Fabien Georget <fabieng@princeton.edu>, Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Princeton University nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
---------------------------------------------------------*/
#include "catch.hpp"
-#include "micpsolver/micpsolver.hpp"
-#include "micpsolver/micpprog.hpp"
-#include "utils/log.hpp"
+#include "specmicp_common/micpsolver/micpsolver.hpp"
+#include "specmicp_common/micpsolver/micpprog.hpp"
+#include "specmicp_common/log.hpp"
using namespace specmicp;
using namespace specmicp::micpsolver;
class TestLinearProgram: public MiCPProg<TestLinearProgram>
{
public:
//! Return the number of variables
int total_variables() {return 3;}
int nb_free_variables() {return 2;}
int nb_complementarity_variables() {return 1;}
//! Return the residual (R(X))
void get_residuals(const Vector& x,
Vector& residual) {
assert(residual.rows() == 3);
residual << 1 + x(0) + x(2), 1 + 1*x(1) + x(2), -x(0) - x(1) +x(2);
}
//! Return the jacobian (J(x))
void get_jacobian(const Vector& x,
Matrix& jacobian)
{
assert(jacobian.cols() == 3);
assert(jacobian.rows() == 3);
jacobian << 1, 0, 1, 0, 1, 1, -1, -1, 1;
}
};
class TestNonLinearProgram: public MiCPProg<TestNonLinearProgram>
{
public:
//! Return the number of variables
int total_variables() {return 3;}
int nb_free_variables() {return 2;}
int nb_complementarity_variables() {return 1;}
//! Return the residual (R(X))
void get_residuals(const Vector& x,
Vector& residual) {
assert(residual.rows() == 3);
residual << -1 + x(0)*x(0) + x(2), 1 + 1*x(1) + x(2)*x(2), -x(0) - x(1) +x(2);
}
//! Return the jacobian (J(x))
void get_jacobian(const Vector& x,
Matrix& jacobian)
{
assert(jacobian.cols() == 3);
assert(jacobian.rows() == 3);
jacobian << 2*x(0), 0, 1, 0, 1, 2*x(2), -1, -1, 1;
}
};
/*
* S. P. Dirkse and M. C. Ferris.
* MCPLIB: a collection of nonlinear mixed complementarity problems.
* Optimization Methods and Software, 5(4):319-345, 1995.
*
*/
class TestKojimaProgram: public MiCPProg<TestKojimaProgram>
{
public:
//! Return the number of variables
int total_variables() {return 4;}
int nb_free_variables() {return 0;}
int nb_complementarity_variables() {return 4;}
//! Return the residual (R(X))
void get_residuals(const Vector& x,
Vector& residual) {
assert(residual.rows() == 4);
residual << 3*x(0)*x(0)+2*x(0)*x(1)+2*x(1)*x(1)+x(2)+3*x(3)-6,
2*x(0)*x(0)+x(1)*x(1)+x(0)+10*x(2) +2*x(3)-2,
3*x(0)+x(0)*x(1)+2*x(1)*x(1)+2*x(2)+9*x(3)-9,
x(0)*x(0)+3*x(1)*x(1)+2*x(2)+3*x(3)-3
;
}
//! Return the jacobian (J(x))
void get_jacobian(Vector& x,
Matrix& jacobian)
{
assert(jacobian.cols() == 4);
assert(jacobian.rows() == 4);
const int neq = total_variables();
Vector res(total_variables());
Vector perturbed_res(total_variables());
get_residuals(x, res);
for (int j=0; j<neq; ++j)
{
double h = 1e-8*std::abs(x(j));
//h = std::copysign(h, x(j));
if (h==0) h = 1e-8;
double tmp = x(j);
x(j) += h;
h = x(j) - tmp;
get_residuals(x, perturbed_res);
for (int i=0; i<neq; ++i)
{
jacobian(i, j) = (perturbed_res(i) - res(i))/h;
}
x(j) = tmp;
}
return;
}
};
TEST_CASE("MiCPSolver")
{
SECTION("linear program 0")
{
std::shared_ptr<TestLinearProgram> ptrprog = std::make_shared<TestLinearProgram>();
MiCPSolver<TestLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 0;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8));
}
SECTION("linear program 10")
{
std::shared_ptr<TestLinearProgram> ptrprog = std::make_shared<TestLinearProgram>();
MiCPSolver<TestLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 10;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8));
}
SECTION("non linear program 0")
{
std::shared_ptr<TestNonLinearProgram> ptrprog = std::make_shared<TestNonLinearProgram>();
MiCPSolver<TestNonLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 0;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8));
}
SECTION("non linear program 5")
{
std::shared_ptr<TestNonLinearProgram> ptrprog = std::make_shared<TestNonLinearProgram>();
MiCPSolver<TestNonLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 5;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8));
}
SECTION("non linear program : Kojima")
{
std::shared_ptr<TestKojimaProgram> ptrprog = std::make_shared<TestKojimaProgram>();
MiCPSolver<TestKojimaProgram> solver(ptrprog);
Eigen::VectorXd x(4);
x << 0.9, 0.1 , 2.9, 0.1;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)-1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)-3) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(3)) == Approx(0.0).epsilon(1e-8));
}
// void test_kojima_program_min()
// {
// std::shared_ptr<TestKojimaProgram> ptrprog = std::make_shared<TestKojimaProgram>();
// MiCPSolver<TestKojimaProgram, NCPfunction::min> solver(ptrprog);
// Eigen::VectorXd x(4);
// x << 0.9, 0.1 , 2.9, 0.1;
// MiCPSolverReturnCode ret = solver.solve(x);
// std::cout << x << std::endl;
// TS_ASSERT_EQUALS(ret, MiCPSolverReturnCode::ResidualMinimized);
// //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(0)+1), 1e-8);
// //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(1)+1), 1e-8);
// //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(2)), 1e-8);
// }
}
diff --git a/tests/micpsolver/micpsolver.cpp b/tests/specmicp_common/micpsolver.cpp.autosave
similarity index 87%
rename from tests/micpsolver/micpsolver.cpp
rename to tests/specmicp_common/micpsolver.cpp.autosave
index 80e5035..3e0e819 100644
--- a/tests/micpsolver/micpsolver.cpp
+++ b/tests/specmicp_common/micpsolver.cpp.autosave
@@ -1,241 +1,265 @@
/*-------------------------------------------------------
- Module : tests/micpsolver
- File : test_micpsolver.hpp
- Author : Fabien Georget
Copyright (c) 2014, Fabien Georget <fabieng@princeton.edu>, Princeton University
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Princeton University nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
---------------------------------------------------------*/
#include "catch.hpp"
-#include "micpsolver/micpsolver.hpp"
-#include "micpsolver/micpprog.hpp"
-#include "utils/log.hpp"
+#include "specmicp_common/micpsolver/micpsolver.hpp"
+#include "specmicp_common/micpsolver/micpprog.hpp"
+#include "specmicp_common/log.hpp"
using namespace specmicp;
using namespace specmicp::micpsolver;
+#include "specmicp_common/micpsolver/ncp_function.hpp"
+
+using specmicp::micpsolver::fisher_burmeister;
+using specmicp::micpsolver::penalized_fisher_burmeister;
+
+TEST_CASE("NCP functions") {
+ SECTION("fisher Burmeister") {
+ CHECK(fisher_burmeister(0.0, 0.0) == 0.0);
+ CHECK(fisher_burmeister(0.0, 1.0) == 0.0);
+ CHECK(fisher_burmeister(0.0, 3.4) == 0.0);
+ CHECK(std::abs(fisher_burmeister(1.0, 1.0)-std::sqrt(2)+2) == Approx(0.0));
+ CHECK(std::abs(fisher_burmeister(2.0, 1.0)-std::sqrt(5)+3) == Approx(0.0));
+ }
+
+ SECTION("Penalized Fischer Burmeister") {
+ CHECK(penalized_fisher_burmeister(0.0, 0.0, 0.5) == 0.0);
+ CHECK(penalized_fisher_burmeister(0.0, 1.0, 0.3) == 0.0);
+ CHECK(penalized_fisher_burmeister(0.0, 3.4, 0.5) == 0.0);
+ CHECK(penalized_fisher_burmeister(8.4, 0.0, 0.2) == 0.0);
+ CHECK(std::abs(fisher_burmeister(1.0, 1.0)-penalized_fisher_burmeister(1.0, 1.0, 1.0)) == Approx(0.0));
+ CHECK(std::abs(penalized_fisher_burmeister(2.0, 1.0, 0.5)-0.5*(std::sqrt(5)-3)+0.5*2.0) == Approx(0.0));
+ }
+}
+
class TestLinearProgram: public MiCPProg<TestLinearProgram>
{
public:
//! Return the number of variables
int total_variables() {return 3;}
int nb_free_variables() {return 2;}
int nb_complementarity_variables() {return 1;}
//! Return the residual (R(X))
void get_residuals(const Vector& x,
Vector& residual) {
assert(residual.rows() == 3);
residual << 1 + x(0) + x(2), 1 + 1*x(1) + x(2), -x(0) - x(1) +x(2);
}
//! Return the jacobian (J(x))
void get_jacobian(const Vector& x,
Matrix& jacobian)
{
assert(jacobian.cols() == 3);
assert(jacobian.rows() == 3);
jacobian << 1, 0, 1, 0, 1, 1, -1, -1, 1;
}
};
class TestNonLinearProgram: public MiCPProg<TestNonLinearProgram>
{
public:
//! Return the number of variables
int total_variables() {return 3;}
int nb_free_variables() {return 2;}
int nb_complementarity_variables() {return 1;}
//! Return the residual (R(X))
void get_residuals(const Vector& x,
Vector& residual) {
assert(residual.rows() == 3);
residual << -1 + x(0)*x(0) + x(2), 1 + 1*x(1) + x(2)*x(2), -x(0) - x(1) +x(2);
}
//! Return the jacobian (J(x))
void get_jacobian(const Vector& x,
Matrix& jacobian)
{
assert(jacobian.cols() == 3);
assert(jacobian.rows() == 3);
jacobian << 2*x(0), 0, 1, 0, 1, 2*x(2), -1, -1, 1;
}
};
/*
* S. P. Dirkse and M. C. Ferris.
* MCPLIB: a collection of nonlinear mixed complementarity problems.
* Optimization Methods and Software, 5(4):319-345, 1995.
*
*/
class TestKojimaProgram: public MiCPProg<TestKojimaProgram>
{
public:
//! Return the number of variables
int total_variables() {return 4;}
int nb_free_variables() {return 0;}
int nb_complementarity_variables() {return 4;}
//! Return the residual (R(X))
void get_residuals(const Vector& x,
Vector& residual) {
assert(residual.rows() == 4);
residual << 3*x(0)*x(0)+2*x(0)*x(1)+2*x(1)*x(1)+x(2)+3*x(3)-6,
2*x(0)*x(0)+x(1)*x(1)+x(0)+10*x(2) +2*x(3)-2,
3*x(0)+x(0)*x(1)+2*x(1)*x(1)+2*x(2)+9*x(3)-9,
x(0)*x(0)+3*x(1)*x(1)+2*x(2)+3*x(3)-3
;
}
//! Return the jacobian (J(x))
void get_jacobian(Vector& x,
Matrix& jacobian)
{
assert(jacobian.cols() == 4);
assert(jacobian.rows() == 4);
const int neq = total_variables();
Vector res(total_variables());
Vector perturbed_res(total_variables());
get_residuals(x, res);
for (int j=0; j<neq; ++j)
{
double h = 1e-8*std::abs(x(j));
//h = std::copysign(h, x(j));
if (h==0) h = 1e-8;
double tmp = x(j);
x(j) += h;
h = x(j) - tmp;
get_residuals(x, perturbed_res);
for (int i=0; i<neq; ++i)
{
jacobian(i, j) = (perturbed_res(i) - res(i))/h;
}
x(j) = tmp;
}
return;
}
};
TEST_CASE("MiCPSolver")
{
SECTION("linear program 0")
{
std::shared_ptr<TestLinearProgram> ptrprog = std::make_shared<TestLinearProgram>();
MiCPSolver<TestLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 0;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8));
}
SECTION("linear program 10")
{
std::shared_ptr<TestLinearProgram> ptrprog = std::make_shared<TestLinearProgram>();
MiCPSolver<TestLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 10;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8));
}
SECTION("non linear program 0")
{
std::shared_ptr<TestNonLinearProgram> ptrprog = std::make_shared<TestNonLinearProgram>();
MiCPSolver<TestNonLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 0;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8));
}
SECTION("non linear program 5")
{
std::shared_ptr<TestNonLinearProgram> ptrprog = std::make_shared<TestNonLinearProgram>();
MiCPSolver<TestNonLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 5;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8));
}
SECTION("non linear program : Kojima")
{
std::shared_ptr<TestKojimaProgram> ptrprog = std::make_shared<TestKojimaProgram>();
MiCPSolver<TestKojimaProgram> solver(ptrprog);
Eigen::VectorXd x(4);
x << 0.9, 0.1 , 2.9, 0.1;
MiCPSolverReturnCode ret = solver.solve(x);
CHECK(ret == MiCPSolverReturnCode::ResidualMinimized);
CHECK(std::abs(x(0)-1) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(1)) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(2)-3) == Approx(0.0).epsilon(1e-8));
CHECK(std::abs(x(3)) == Approx(0.0).epsilon(1e-8));
}
// void test_kojima_program_min()
// {
// std::shared_ptr<TestKojimaProgram> ptrprog = std::make_shared<TestKojimaProgram>();
// MiCPSolver<TestKojimaProgram, NCPfunction::min> solver(ptrprog);
// Eigen::VectorXd x(4);
// x << 0.9, 0.1 , 2.9, 0.1;
// MiCPSolverReturnCode ret = solver.solve(x);
// std::cout << x << std::endl;
// TS_ASSERT_EQUALS(ret, MiCPSolverReturnCode::ResidualMinimized);
// //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(0)+1), 1e-8);
// //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(1)+1), 1e-8);
// //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(2)), 1e-8);
// }
}
diff --git a/tests/common/misc.cpp b/tests/specmicp_common/misc.cpp
similarity index 100%
rename from tests/common/misc.cpp
rename to tests/specmicp_common/misc.cpp
diff --git a/tests/common/plugin_manager.cpp b/tests/specmicp_common/plugin_manager.cpp
similarity index 100%
rename from tests/common/plugin_manager.cpp
rename to tests/specmicp_common/plugin_manager.cpp
diff --git a/tests/common/range_iterator.cpp b/tests/specmicp_common/range_iterator.cpp
similarity index 100%
rename from tests/common/range_iterator.cpp
rename to tests/specmicp_common/range_iterator.cpp
diff --git a/tests/common/sparse_solvers.cpp b/tests/specmicp_common/sparse_solvers.cpp
similarity index 100%
rename from tests/common/sparse_solvers.cpp
rename to tests/specmicp_common/sparse_solvers.cpp
diff --git a/tests/common/test_basic_plugin.cpp b/tests/specmicp_common/test_basic_plugin.cpp
similarity index 100%
rename from tests/common/test_basic_plugin.cpp
rename to tests/specmicp_common/test_basic_plugin.cpp
diff --git a/tests/common/test_basic_plugin_mockobject.hpp b/tests/specmicp_common/test_basic_plugin_mockobject.hpp
similarity index 100%
rename from tests/common/test_basic_plugin_mockobject.hpp
rename to tests/specmicp_common/test_basic_plugin_mockobject.hpp
diff --git a/tests/common/test_common.cpp b/tests/specmicp_common/test_common.cpp
similarity index 100%
rename from tests/common/test_common.cpp
rename to tests/specmicp_common/test_common.cpp
diff --git a/tests/common/test_to_load.cpp b/tests/specmicp_common/test_to_load.cpp
similarity index 100%
rename from tests/common/test_to_load.cpp
rename to tests/specmicp_common/test_to_load.cpp
diff --git a/tests/common/units.cpp b/tests/specmicp_common/units.cpp
similarity index 100%
rename from tests/common/units.cpp
rename to tests/specmicp_common/units.cpp
diff --git a/tests/common/value_checker.cpp b/tests/specmicp_common/value_checker.cpp
similarity index 100%
rename from tests/common/value_checker.cpp
rename to tests/specmicp_common/value_checker.cpp
diff --git a/tests/common/vector_checker.cpp b/tests/specmicp_common/vector_checker.cpp
similarity index 100%
rename from tests/common/vector_checker.cpp
rename to tests/specmicp_common/vector_checker.cpp
diff --git a/tests/common/yaml.cpp b/tests/specmicp_common/yaml.cpp
similarity index 100%
rename from tests/common/yaml.cpp
rename to tests/specmicp_common/yaml.cpp