diff --git a/CMakeLists.txt b/CMakeLists.txt
index 95692171b86e85ab125b39451f7f7849de3920c3..ee1f86f5a773b12a1e27181afb6bd3173d7ba60e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/cpack.cmake)
-
+INCLUDE(cmake2/FindqpOASES.cmake)
SET(PROJECT_NAME robust-equilibrium-lib)
SET(PROJECT_DESCRIPTION
@@ -36,6 +36,7 @@ SET(${PROJECT_NAME}_HEADERS
include/robust-equilibrium-lib/solver_LP_qpoases.hh
include/robust-equilibrium-lib/solver_LP_clp.hh
include/robust-equilibrium-lib/static_equilibrium.hh
+ include/robust-equilibrium-lib/stop-watch.hh
)
find_package(Eigen3)
@@ -48,6 +49,7 @@ endif()
find_package(CDD REQUIRED)
find_package(CLP REQUIRED)
+SEARCH_FOR_QPOASES()
add_subdirectory (src)
add_subdirectory (test)
diff --git a/cmake2/FindqpOASES.cmake b/cmake2/FindqpOASES.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..0f349c647571c52100c0f840c4a1faa54b82f1b7
--- /dev/null
+++ b/cmake2/FindqpOASES.cmake
@@ -0,0 +1,53 @@
+#.rst:
+# FindqpOASES
+# -----------
+#
+# Try to find the qpOASES library.
+# Once done this will define the following variables::
+#
+# qpOASES_FOUND - System has qpOASES
+# qpOASES_INCLUDE_DIR - qpOASES include directory
+# qpOASES_LIBRARY - qpOASES libraries
+#
+# qpOASES does not have an "install" step, and the includes are in the source
+# tree, while the libraries are in the build tree.
+# Therefore the environment and cmake variables `qpOASES_SOURCE_DIR` and
+# `qpOASES_BINARY_DIR` will be used to locate the includes and libraries.
+
+#=============================================================================
+# Copyright 2014 iCub Facility, Istituto Italiano di Tecnologia
+# Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
+#
+# Distributed under the OSI-approved BSD License (the "License");
+# see accompanying file Copyright.txt for details.
+#
+# This software is distributed WITHOUT ANY WARRANTY; without even the
+# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the License for more information.
+#=============================================================================
+# (To distribute this file outside of YCM, substitute the full
+# License text for the above reference.)
+MACRO(SEARCH_FOR_QPOASES)
+
+include(FindPackageHandleStandardArgs)
+
+find_path(qpOASES_INCLUDEDIR
+ NAMES qpOASES.hpp
+ HINTS "${qpOASES_SOURCE_DIR}"
+ ENV qpOASES_SOURCE_DIR
+ PATH_SUFFIXES include)
+find_library(qpOASES_LIB
+ NAMES qpOASES
+ HINTS "${qpOASES_BINARY_DIR}"
+ ENV qpOASES_BINARY_DIR
+ PATH_SUFFIXES lib
+ libs)
+
+set(qpOASES_INCLUDE_DIR ${qpOASES_INCLUDEDIR})
+set(qpOASES_LIBRARY ${qpOASES_LIB})
+
+find_package_handle_standard_args(qpOASES DEFAULT_MSG qpOASES_LIBRARY
+ qpOASES_INCLUDE_DIR)
+set(qpOASES_FOUND ${QPOASES_FOUND})
+
+ENDMACRO(SEARCH_FOR_QPOASES)
diff --git a/include/robust-equilibrium-lib/Stdafx.hh b/include/robust-equilibrium-lib/Stdafx.hh
new file mode 100644
index 0000000000000000000000000000000000000000..8e7b109b8e1346c59dd69a2a7bc3f3d14afbfb7d
--- /dev/null
+++ b/include/robust-equilibrium-lib/Stdafx.hh
@@ -0,0 +1,32 @@
+/*
+Copyright (c) 2010-2013 Tommaso Urli
+
+Tommaso Urli tommaso.urli@uniud.it University of Udine
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the
+"Software"), to deal in the Software without restriction, including
+without limitation the rights to use, copy, modify, merge, publish,
+distribute, sublicense, and/or sell copies of the Software, and to
+permit persons to whom the Software is furnished to do so, subject to
+the following conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+*/
+
+#pragma once
+
+#include <iostream>
+#include <map>
+#include <ctime>
+#include <sstream>
diff --git a/include/robust-equilibrium-lib/logger.hh b/include/robust-equilibrium-lib/logger.hh
index 98180c88a7b1ad86270d5044e103c052e6865443..565d9c3c088a5a500d29569ade9ffcd2b61aab78 100644
--- a/include/robust-equilibrium-lib/logger.hh
+++ b/include/robust-equilibrium-lib/logger.hh
@@ -12,6 +12,7 @@
#include <robust-equilibrium-lib/config.hh>
#include <sstream>
+#include <Eigen/Dense>
#include <map>
#include <initializer_list>
#include "boost/assign.hpp"
@@ -100,6 +101,16 @@ namespace robust_equilibrium
return ss.str();
}
+ template<typename T, int n>
+ std::string toString(const Eigen::MatrixBase<T>& v, const std::string separator=", ")
+ {
+ if(v.rows()>v.cols())
+ return toString(v.transpose(), separator);
+ std::stringstream ss;
+ ss<<v;
+ return ss.str();
+ }
+
enum ROBUST_EQUILIBRIUM_DLLAPI LoggerVerbosity
{
VERBOSITY_ALL,
diff --git a/include/robust-equilibrium-lib/solver_LP_abstract.hh b/include/robust-equilibrium-lib/solver_LP_abstract.hh
index 9f03bbe45a10f93fd3f1ebaeb671a2184df6fe97..f807b4c4d5049235caa14f6e4673686e53660d14 100644
--- a/include/robust-equilibrium-lib/solver_LP_abstract.hh
+++ b/include/robust-equilibrium-lib/solver_LP_abstract.hh
@@ -52,6 +52,10 @@ public:
/** Get the objective value of the last solved problem. */
virtual double getObjectiveValue() = 0;
+ virtual void getDualSolution(Ref_vectorX res) = 0;
+
+// virtual void getDualColumnSolution(Ref_vectorX res) = 0;
+
/** Get the current maximum number of iterations performed
* by the solver.
*/
diff --git a/include/robust-equilibrium-lib/solver_LP_clp.hh b/include/robust-equilibrium-lib/solver_LP_clp.hh
index 2478a43c180a1db6ae7a4132982acc7082d8cb7a..cc39625294420276314dffadbd501f87a2c6131d 100644
--- a/include/robust-equilibrium-lib/solver_LP_clp.hh
+++ b/include/robust-equilibrium-lib/solver_LP_clp.hh
@@ -38,6 +38,10 @@ public:
/** Get the objective value of the last solved problem. */
double getObjectiveValue();
+ void getDualSolution(Ref_vectorX res);
+
+// void getDualColumnSolution(Ref_vectorX res);
+
/** Get the current maximum number of iterations performed
* by the solver.
*/
diff --git a/include/robust-equilibrium-lib/solver_LP_qpoases.hh b/include/robust-equilibrium-lib/solver_LP_qpoases.hh
index ed3b1456d385423d3b32b4006b41de87c0547acb..70c99854b0e2a03f782ebb8194558cb7ff480066 100644
--- a/include/robust-equilibrium-lib/solver_LP_qpoases.hh
+++ b/include/robust-equilibrium-lib/solver_LP_qpoases.hh
@@ -2,3 +2,68 @@
* Copyright 2015, LAAS-CNRS
* Author: Andrea Del Prete
*/
+
+#ifndef ROBUST_EQUILIBRIUM_LIB_SOLVER_QPOASES_HH
+#define ROBUST_EQUILIBRIUM_LIB_SOLVER_QPOASES_HH
+
+#include <robust-equilibrium-lib/config.hh>
+#include <robust-equilibrium-lib/util.hh>
+#include <robust-equilibrium-lib/solver_LP_abstract.hh>
+#include <qpOASES.hpp>
+
+namespace robust_equilibrium
+{
+
+class ROBUST_EQUILIBRIUM_DLLAPI Solver_LP_qpoases: public Solver_LP_abstract
+{
+private:
+ qpOASES::Options m_options; // solver options
+ qpOASES::SQProblem m_solver; // qpoases solver
+
+ MatrixXX m_H; // Hessian matrix
+ bool m_init_succeeded; // true if solver has been successfully initialized
+ qpOASES::returnValue m_status; // status code returned by the solver
+ int m_maxIter; // max number of iterations
+ double m_maxTime; // max time to solve the LP [s]
+
+public:
+
+ Solver_LP_qpoases();
+
+ /** Solve the linear program
+ * minimize c' x
+ * subject to Alb <= A x <= Aub
+ * lb <= x <= ub
+ */
+ LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
+ Ref_vectorX sol);
+
+ /** Get the status of the solver. */
+ LP_status getStatus();
+
+ /** Get the objective value of the last solved problem. */
+ double getObjectiveValue();
+
+ void getDualSolution(Ref_vectorX res);
+
+// void getDualColumnSolution(Ref_vectorX res);
+
+ /** Get the current maximum number of iterations performed
+ * by the solver.
+ */
+ unsigned int getMaximumIterations();
+
+ /** Set the current maximum number of iterations performed
+ * by the solver.
+ */
+ bool setMaximumIterations(unsigned int maxIter);
+
+ /** Set the maximum time allowed to solve a problem. */
+ bool setMaximumTime(double seconds);
+
+};
+
+} // end namespace robust_equilibrium
+
+#endif //ROBUST_EQUILIBRIUM_LIB_SOLVER_QPOASES_HH
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 8c1e94ea8d0253d3e05dfba0693a5d5ec0632c94..e3825a1cf0bb96a65d7c4f32bfc4734447fb85f5 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -17,6 +17,8 @@ namespace robust_equilibrium
enum ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibriumAlgorithm
{
STATIC_EQUILIBRIUM_ALGORITHM_LP,
+ STATIC_EQUILIBRIUM_ALGORITHM_LP2,
+ STATIC_EQUILIBRIUM_ALGORITHM_DLP,
STATIC_EQUILIBRIUM_ALGORITHM_PP,
STATIC_EQUILIBRIUM_ALGORITHM_IP,
STATIC_EQUILIBRIUM_ALGORITHM_DIP
diff --git a/include/robust-equilibrium-lib/stop-watch.hh b/include/robust-equilibrium-lib/stop-watch.hh
new file mode 100644
index 0000000000000000000000000000000000000000..b86cfeb06ec377f8dfd5967a1c50b4082c852c2b
--- /dev/null
+++ b/include/robust-equilibrium-lib/stop-watch.hh
@@ -0,0 +1,267 @@
+/*
+Copyright (c) 2010-2013 Tommaso Urli
+
+Tommaso Urli tommaso.urli@uniud.it University of Udine
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the
+"Software"), to deal in the Software without restriction, including
+without limitation the rights to use, copy, modify, merge, publish,
+distribute, sublicense, and/or sell copies of the Software, and to
+permit persons to whom the Software is furnished to do so, subject to
+the following conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+*/
+
+
+#ifndef WBR_STOPWATCH_H
+#define WBR_STOPWATCH_H
+
+#include "robust-equilibrium-lib/Stdafx.hh"
+
+#ifndef WIN32
+ /* The classes below are exported */
+ #pragma GCC visibility push(default)
+#endif
+
+// Generic stopwatch exception class
+struct StopwatchException
+{
+public:
+ StopwatchException(std::string error) : error(error) { }
+ std::string error;
+};
+
+
+enum StopwatchMode
+{
+ NONE = 0, // Clock is not initialized
+ CPU_TIME = 1, // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
+ REAL_TIME = 2 // Clock calculates time by asking the operating system how
+ // much real time passed
+};
+
+/**
+ @brief A class representing a stopwatch.
+
+ @code
+ Stopwatch swatch();
+ @endcode
+
+ The Stopwatch class can be used to measure execution time of code,
+ algorithms, etc., // TODO: he Stopwatch can be initialized in two
+ time-taking modes, CPU time and real time:
+
+ @code
+ swatch.set_mode(REAL_TIME);
+ @endcode
+
+ CPU time is the time spent by the processor on a certain piece of code,
+ while real time is the real amount of time taken by a certain piece of
+ code to execute (i.e. in general if you are doing hard work such as
+ image or video editing on a different process the measured time will
+ probably increase).
+
+ How does it work? Basically, one wraps the code to be measured with the
+ following method calls:
+
+ @code
+ swatch.start("My astounding algorithm");
+ // Hic est code
+ swatch.stop("My astounding algorithm");
+ @endcode
+
+ A string representing the code ID is provided so that nested portions of
+ code can be profiled separately:
+
+ @code
+ swatch.start("My astounding algorithm");
+
+ swatch.start("My astounding algorithm - Super smart init");
+ // Initialization
+ swatch.stop("My astounding algorithm - Super smart init");
+
+ swatch.start("My astounding algorithm - Main loop");
+ // Loop
+ swatch.stop("My astounding algorithm - Main loop");
+
+ swatch.stop("My astounding algorithm");
+ @endcode
+
+ Note: ID strings can be whatever you like, in the previous example I have
+ used "My astounding algorithm - *" only to enforce the fact that the
+ measured code portions are part of My astounding algorithm, but there's no
+ connection between the three measurements.
+
+ If the code for a certain task is scattered through different files or
+ portions of the same file one can use the start-pause-stop method:
+
+ @code
+ swatch.start("Setup");
+ // First part of setup
+ swatch.pause("Setup");
+
+ swatch.start("Main logic");
+ // Main logic
+ swatch.stop("Main logic");
+
+ swatch.start("Setup");
+ // Cleanup (part of the setup)
+ swatch.stop("Setup");
+ @endcode
+
+ Finally, to report the results of the measurements just run:
+
+ @code
+ swatch.report("Code ID");
+ @endcode
+
+ Thou can also provide an additional std::ostream& parameter to report() to
+ redirect the logging on a different output. Also, you can use the
+ get_total/min/max/average_time() methods to get the individual numeric data,
+ without all the details of the logging. You can also extend Stopwatch to
+ implement your own logging syntax.
+
+ To report all the measurements:
+
+ @code
+ swatch.report_all();
+ @endcode
+
+ Same as above, you can redirect the output by providing a std::ostream&
+ parameter.
+
+*/
+class Stopwatch {
+public:
+
+ /** Constructor */
+ Stopwatch(StopwatchMode _mode=NONE);
+
+ /** Destructor */
+ ~Stopwatch();
+
+ /** Tells if a performance with a certain ID exists */
+ bool performance_exists(std::string perf_name);
+
+ /** Initialize stopwatch to use a certain time taking mode */
+ void set_mode(StopwatchMode mode);
+
+ /** Start the stopwatch related to a certain piece of code */
+ void start(std::string perf_name);
+
+ /** Stops the stopwatch related to a certain piece of code */
+ void stop(std::string perf_name);
+
+ /** Stops the stopwatch related to a certain piece of code */
+ void pause(std::string perf_name);
+
+ /** Reset a certain performance record */
+ void reset(std::string perf_name);
+
+ /** Resets all the performance records */
+ void reset_all();
+
+ /** Dump the data of a certain performance record */
+ void report(std::string perf_name, int precision=2,
+ std::ostream& output = std::cout);
+
+ /** Dump the data of all the performance records */
+ void report_all(int precision=2, std::ostream& output = std::cout);
+
+ /** Returns total execution time of a certain performance */
+ long double get_total_time(std::string perf_name);
+
+ /** Returns average execution time of a certain performance */
+ long double get_average_time(std::string perf_name);
+
+ /** Returns minimum execution time of a certain performance */
+ long double get_min_time(std::string perf_name);
+
+ /** Returns maximum execution time of a certain performance */
+ long double get_max_time(std::string perf_name);
+
+ /** Return last measurement of a certain performance */
+ long double get_last_time(std::string perf_name);
+
+ /** Return the time since the start of the last measurement of a given
+ performance. */
+ long double get_time_so_far(std::string perf_name);
+
+ /** Turn off clock, all the Stopwatch::* methods return without doing
+ anything after this method is called. */
+ void turn_off();
+
+ /** Turn on clock, restore clock operativity after a turn_off(). */
+ void turn_on();
+
+ /** Take time, depends on mode */
+ long double take_time();
+
+protected:
+
+ /** Struct to hold the performance data */
+ struct PerformanceData {
+
+ PerformanceData() :
+ clock_start(0),
+ total_time(0),
+ min_time(0),
+ max_time(0),
+ last_time(0),
+ paused(false),
+ stops(0) {
+ }
+
+ /** Start time */
+ long double clock_start;
+
+ /** Cumulative total time */
+ long double total_time;
+
+ /** Minimum time */
+ long double min_time;
+
+ /** Maximum time */
+ long double max_time;
+
+ /** Last time */
+ long double last_time;
+
+ /** Tells if this performance has been paused, only for internal use */
+ bool paused;
+
+ /** How many cycles have been this stopwatch executed? */
+ int stops;
+ };
+
+ /** Flag to hold the clock's status */
+ bool active;
+
+ /** Time taking mode */
+ StopwatchMode mode;
+
+ /** Pointer to the dynamic structure which holds the collection of performance
+ data */
+ std::map<std::string, PerformanceData >* records_of;
+
+};
+
+Stopwatch& getProfiler();
+
+#ifndef WIN32
+ #pragma GCC visibility pop
+#endif
+
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1be3e52de65d24a33e92392652412e0fe973a3d2..183f0476b82245784a6a263f498f471dcad8b643 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -5,6 +5,7 @@ include_directories("${INCLUDE_DIR}")
include_directories("${EIGEN3_INCLUDE_DIR}")
include_directories("${CDD_INCLUDE_DIR}")
include_directories("${CLP_INCLUDE_DIR}")
+include_directories("${qpOASES_INCLUDE_DIR}")
SET(LIBRARY_NAME ${PROJECT_NAME})
@@ -20,6 +21,7 @@ SET(${LIBRARY_NAME}_SOURCES
solver_LP_clp.cpp
util.cpp
logger.cpp
+ stop-watch.cpp
)
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
@@ -30,6 +32,7 @@ if ( MSVC )
endif ( MSVC )
TARGET_LINK_LIBRARIES(robust-equilibrium-lib ${CDD_LIBRARY}
+ ${qpOASES_LIBRARY}
${CLP_LIBRARY}
/usr/lib/libCoinUtils.so)
diff --git a/src/solver_LP_abstract.cpp b/src/solver_LP_abstract.cpp
index bad6cbac17e8288907aee7b2636c3a92aa405f44..8ea2808f3e8ce83daef328c095df757ea68a65f9 100644
--- a/src/solver_LP_abstract.cpp
+++ b/src/solver_LP_abstract.cpp
@@ -5,6 +5,7 @@
#include <robust-equilibrium-lib/solver_LP_abstract.hh>
#include <robust-equilibrium-lib/solver_LP_clp.hh>
+#include <robust-equilibrium-lib/solver_LP_qpoases.hh>
#include <robust-equilibrium-lib/logger.hh>
#include <iostream>
@@ -17,6 +18,8 @@ Solver_LP_abstract* Solver_LP_abstract::getNewSolver(SolverLP solverType)
{
if(solverType==SOLVER_LP_CLP)
return new Solver_LP_clp();
+ if(solverType==SOLVER_LP_QPOASES)
+ return new Solver_LP_qpoases();
SEND_ERROR_MSG("Specified solver type not recognized: "+toString(solverType));
return NULL;
diff --git a/src/solver_LP_clp.cpp b/src/solver_LP_clp.cpp
index cb3a0b8e76f47b8920fd8efa4c2ee6a3b82761a0..24aa67d84f8c2ddbd1078f990435cf59ce8e4249 100644
--- a/src/solver_LP_clp.cpp
+++ b/src/solver_LP_clp.cpp
@@ -77,6 +77,20 @@ double Solver_LP_clp::getObjectiveValue()
return m_model.objectiveValue();
}
+void Solver_LP_clp::getDualSolution(Ref_vectorX res)
+{
+ const double *tmp = m_model.dualRowSolution();
+ for(int i=0; i<res.size(); i++)
+ res(i) = tmp[i];
+}
+
+//void Solver_LP_clp::getDualColumnSolution(Ref_vectorX res)
+//{
+// const double *tmp = m_model.dualColumnSolution();
+// for(int i=0; i<res.size(); i++)
+// res(i) = tmp[i];
+//}
+
unsigned int Solver_LP_clp::getMaximumIterations()
{
int integerValue;
diff --git a/src/solver_LP_qpoases.cpp b/src/solver_LP_qpoases.cpp
index ed3b1456d385423d3b32b4006b41de87c0547acb..aef5c2e2c9302ccf1695cb15a7cb6444893b9af3 100644
--- a/src/solver_LP_qpoases.cpp
+++ b/src/solver_LP_qpoases.cpp
@@ -2,3 +2,122 @@
* Copyright 2015, LAAS-CNRS
* Author: Andrea Del Prete
*/
+
+#include <robust-equilibrium-lib/solver_LP_qpoases.hh>
+#include <robust-equilibrium-lib/logger.hh>
+
+USING_NAMESPACE_QPOASES
+
+namespace robust_equilibrium
+{
+
+ Solver_LP_qpoases::Solver_LP_qpoases(): Solver_LP_abstract()
+ {
+// m_options.initialStatusBounds = ST_INACTIVE;
+// m_options.setToReliable();
+ m_options.setToDefault();
+ m_options.printLevel = PL_LOW; //PL_NONE
+ m_options.enableRegularisation = BT_TRUE;
+ m_options.enableEqualities = BT_TRUE;
+ m_maxIter = 1000;
+ m_maxTime = 100.0;
+ }
+
+ LP_status Solver_LP_qpoases::solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
+ Ref_vectorX sol)
+ {
+ int n = (int)c.size(); // number of variables
+ int m = (int)A.rows(); // number of constraints
+ assert(lb.size()==n);
+ assert(ub.size()==n);
+ assert(A.cols()==n);
+ assert(Alb.size()==m);
+ assert(Aub.size()==m);
+
+ int iters = m_maxIter;
+ double solutionTime = m_maxTime;
+ if(n!=m_solver.getNV() || m!=m_solver.getNC())
+ {
+ m_solver = SQProblem(n, m, HST_ZERO);
+ m_solver.setOptions(m_options);
+// m_solver.printOptions();
+ m_init_succeeded = false;
+ m_H = MatrixXX::Zero(n,n);
+ }
+
+// m_status = m_solver.init(NULL, c.data(), A.data(), lb.data(), ub.data(),
+// Alb.data(), Aub.data(), iters, &solutionTime);
+
+ if(!m_init_succeeded)
+ {
+ m_status = m_solver.init(NULL, c.data(), A.data(), lb.data(), ub.data(),
+ Alb.data(), Aub.data(), iters, &solutionTime);
+ if(m_status==SUCCESSFUL_RETURN)
+ m_init_succeeded = true;
+ }
+ else
+ {
+ // this doesn't work if I pass NULL instead of m_H.data()
+ m_status = m_solver.hotstart(m_H.data(), c.data(), A.data(), lb.data(), ub.data(),
+ Alb.data(), Aub.data(), iters, &solutionTime);
+ if(m_status!=SUCCESSFUL_RETURN)
+ m_init_succeeded = false;
+ }
+
+ if(m_status==SUCCESSFUL_RETURN)
+ {
+ m_solver.getPrimalSolution(sol.data());
+ }
+ else
+ SEND_DEBUG_MSG("QPoases solver failed with code "+toString(m_status));
+
+ return getStatus();
+ }
+
+ LP_status Solver_LP_qpoases::getStatus()
+ {
+ int ss = getSimpleStatus(m_status);
+ if(ss==0)
+ return LP_STATUS_OPTIMAL;
+ if(ss==1)
+ return LP_STATUS_MAX_ITER_REACHED;
+ if(ss==-2)
+ return LP_STATUS_INFEASIBLE;
+ if(ss==-3)
+ return LP_STATUS_DUAL_INFEASIBLE;
+ return LP_STATUS_ERROR;
+ }
+
+ double Solver_LP_qpoases::getObjectiveValue()
+ {
+ return m_solver.getObjVal();
+ }
+
+ void Solver_LP_qpoases::getDualSolution(Ref_vectorX res)
+ {
+ m_solver.getDualSolution(res.data());
+ }
+
+ unsigned int Solver_LP_qpoases::getMaximumIterations()
+ {
+ return m_maxIter;
+ }
+
+ bool Solver_LP_qpoases::setMaximumIterations(unsigned int maxIter)
+ {
+ if(maxIter==0)
+ return false;
+ m_maxIter = maxIter;
+ return true;
+ }
+
+ bool Solver_LP_qpoases::setMaximumTime(double seconds)
+ {
+ if(seconds<=0.0)
+ return false;
+ m_maxTime = seconds;
+ return true;
+ }
+
+} // end namespace robust_equilibrium
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 3200612c0488c369960ba9255a0c0558b66e72b8..d7697a30dece442e00621fc9d462d04a74361b5a 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -23,6 +23,12 @@ StaticEquilibrium::StaticEquilibrium(double mass, unsigned int generatorsPerCont
m_is_cdd_initialized = true;
}
+ if(generatorsPerContact!=4)
+ {
+ SEND_WARNING_MSG("Algorithm currently supports only 4 generators per contact!");
+ generatorsPerContact = 4;
+ }
+
m_solver = Solver_LP_abstract::getNewSolver(solver_type);
m_generatorsPerContact = generatorsPerContact;
@@ -121,53 +127,15 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
return true;
}
+
double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
{
- /*Compute the robustness measure of the equilibrium of a specified CoM position.
- The operation amounts to solving the following dual LP:
- find v
- minimize (d+D*com)' v
- subject to G' v >= 0
- 1' G' v = 1
- where
- -(d+D c)' v is the robustness measure
- c is the CoM position
- G is the matrix whose columns are the gravito-inertial wrench generators
- */
- if(m_algorithm!=STATIC_EQUILIBRIUM_ALGORITHM_LP)
- {
- SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the LP algorithm");
- return false;
- }
-
- double robustness_dual, robustness_primal;
- LP_status lpStatus_primal, lpStatus_dual;
- long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
- Vector6 v;
- VectorX b_b0(m+1);
+ const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_LP)
{
-
- Vector6 c = m_D*com + m_d;
- Vector6 lb = Vector6::Ones()*-1e100;
- Vector6 ub = Vector6::Ones()*1e100;
- VectorX Alb = VectorX::Zero(m+1);
- Alb(m) = 1.0;
- VectorX Aub = VectorX::Ones(m+1)*1e100;
- Aub(m) = 1.0;
- MatrixX6 A(m+1,6);
- A.topRows(m) = m_G_centr.transpose();
- A.bottomRows<1>() = (m_G_centr*VectorX::Ones(m)).transpose();
-
- lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
- if(lpStatus_dual==LP_STATUS_OPTIMAL)
- robustness_dual = m_solver->getObjectiveValue();
- else
- SEND_ERROR_MSG("Dual LP problem could not be solved: "+toString(lpStatus_dual));
- }
-
- {
- /* The operation amounts to solving the following LP:
+ /* Compute the robustness measure of the equilibrium of a specified CoM position
+ * by solving the following LP:
find b, b0
minimize -b0
subject to D c + d <= G b <= D c + d
@@ -178,10 +146,10 @@ double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
c is the CoM position
G is the matrix whose columns are the gravito-inertial wrench generators
*/
- long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ VectorX b_b0(m+1);
VectorX c = VectorX::Zero(m+1);
c(m) = -1.0;
- VectorX lb = -VectorX::Ones(m+1)*1e10;
+ VectorX lb = -VectorX::Ones(m+1)*1e5;
VectorX ub = VectorX::Ones(m+1)*1e10;
VectorX Alb = VectorX::Zero(6+m);
VectorX Aub = VectorX::Ones(6+m)*1e100;
@@ -192,21 +160,84 @@ double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
A.bottomLeftCorner(m,m) = MatrixXX::Identity(m,m);
A.bottomRightCorner(m,1) = -VectorX::Ones(m);
- lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
+ LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
+ if(lpStatus_primal==LP_STATUS_OPTIMAL)
+ return -1.0*m_solver->getObjectiveValue();
+
+ SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+ return -1.0;
+ }
+
+ if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_LP2)
+ {
+ /* Compute the robustness measure of the equilibrium of a specified CoM position
+ * by solving the following LP:
+ find b, b0
+ minimize -b0
+ subject to D c + d <= G (b + 1*b0) <= D c + d
+ 0 <= b <= Inf
+ where
+ b are the (delta) coefficient of the contact force generators (f = G (b+b0))
+ b0 is the robustness measure
+ c is the CoM position
+ G is the matrix whose columns are the gravito-inertial wrench generators
+ */
+ VectorX b_b0(m+1);
+ VectorX c = VectorX::Zero(m+1);
+ c(m) = -1.0;
+ VectorX lb = VectorX::Zero(m+1);
+ lb(m) = -1e10;
+ VectorX ub = VectorX::Ones(m+1)*1e10;
+ MatrixXX A = MatrixXX::Zero(6, m+1);
+ Vector6 Alb = m_D * com + m_d;
+ Vector6 Aub = Alb;
+ A.leftCols(m) = m_G_centr;
+ A.rightCols(1) = m_G_centr * VectorX::Ones(m);
+
+ LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
if(lpStatus_primal==LP_STATUS_OPTIMAL)
- robustness_primal = -1.0*m_solver->getObjectiveValue();
- else
- SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+ {
+ return -1.0*m_solver->getObjectiveValue();
+ }
+ SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+ return -1.0;
}
-// if(lpStatus_primal==LP_STATUS_OPTIMAL && lpStatus_dual==LP_STATUS_OPTIMAL)
-// {
-// if(fabs(robustness_dual-robustness_primal)>1e-3)
-// SEND_ERROR_MSG("Primal and dual solutions differ: "+toString(robustness_primal)+" != "+
-// toString(robustness_dual)+" b="+toString(b_b0.transpose()));
-// }
+ if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_DLP)
+ {
+ /*Compute the robustness measure of the equilibrium of a specified CoM position
+ by solving the following dual LP:
+ find v
+ minimize (d+D*com)' v
+ subject to G' v >= 0
+ 1' G' v = 1
+ where
+ -(d+D c)' v is the robustness measure
+ c is the CoM position
+ G is the matrix whose columns are the gravito-inertial wrench generators
+ */
+ Vector6 v;
+ Vector6 c = m_D*com + m_d;
+ Vector6 lb = Vector6::Ones()*-1e100;
+ Vector6 ub = Vector6::Ones()*1e100;
+ VectorX Alb = VectorX::Zero(m+1);
+ Alb(m) = 1.0;
+ VectorX Aub = VectorX::Ones(m+1)*1e100;
+ Aub(m) = 1.0;
+ MatrixX6 A(m+1,6);
+ A.topRows(m) = m_G_centr.transpose();
+ A.bottomRows<1>() = (m_G_centr*VectorX::Ones(m)).transpose();
+
+ LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
+ if(lpStatus_dual==LP_STATUS_OPTIMAL)
+ return m_solver->getObjectiveValue();
+
+ SEND_ERROR_MSG("Dual LP problem could not be solved: "+toString(lpStatus_dual));
+ return -1.0;
+ }
- return robustness_primal;
+ SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the LP algorithm");
+ return -1.0;
}
bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
diff --git a/src/stop-watch.cpp b/src/stop-watch.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e2c8d4598ce26d3c05faa7a9366cf08b3a6e5ea7
--- /dev/null
+++ b/src/stop-watch.cpp
@@ -0,0 +1,341 @@
+/*
+Copyright (c) 2010-2013 Tommaso Urli
+
+Tommaso Urli tommaso.urli@uniud.it University of Udine
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the
+"Software"), to deal in the Software without restriction, including
+without limitation the rights to use, copy, modify, merge, publish,
+distribute, sublicense, and/or sell copies of the Software, and to
+permit persons to whom the Software is furnished to do so, subject to
+the following conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+*/
+
+#include "sot/torque_control/utils/Stdafx.hh"
+
+#ifndef WIN32
+ #include <sys/time.h>
+#else
+ #include <Windows.h>
+ #include <iomanip>
+#endif
+
+#include <iomanip> // std::setprecision
+#include "sot/torque_control/utils/stop-watch.hh"
+
+using std::map;
+using std::string;
+using std::ostringstream;
+
+Stopwatch& getProfiler()
+{
+ static Stopwatch s(REAL_TIME); // alternatives are CPU_TIME and REAL_TIME
+ return s;
+}
+
+Stopwatch::Stopwatch(StopwatchMode _mode)
+ : active(true), mode(_mode)
+{
+ records_of = new map<string, PerformanceData>();
+}
+
+Stopwatch::~Stopwatch()
+{
+ delete records_of;
+}
+
+void Stopwatch::set_mode(StopwatchMode new_mode)
+{
+ mode = new_mode;
+}
+
+bool Stopwatch::performance_exists(string perf_name)
+{
+ return (records_of->find(perf_name) != records_of->end());
+}
+
+long double Stopwatch::take_time()
+{
+ if ( mode == CPU_TIME ) {
+
+ // Use ctime
+ return clock();
+
+ } else if ( mode == REAL_TIME ) {
+
+ // Query operating system
+
+#ifdef WIN32
+ /* In case of usage under Windows */
+ FILETIME ft;
+ LARGE_INTEGER intervals;
+
+ // Get the amount of 100 nanoseconds intervals elapsed since January 1, 1601
+ // (UTC)
+ GetSystemTimeAsFileTime(&ft);
+ intervals.LowPart = ft.dwLowDateTime;
+ intervals.HighPart = ft.dwHighDateTime;
+
+ long double measure = intervals.QuadPart;
+ measure -= 116444736000000000.0; // Convert to UNIX epoch time
+ measure /= 10000000.0; // Convert to seconds
+
+ return measure;
+#else
+ /* Linux, MacOS, ... */
+ struct timeval tv;
+ gettimeofday(&tv, NULL);
+
+ long double measure = tv.tv_usec;
+ measure /= 1000000.0; // Convert to seconds
+ measure += tv.tv_sec; // Add seconds part
+
+ return measure;
+#endif
+
+ } else {
+ // If mode == NONE, clock has not been initialized, then throw exception
+ throw StopwatchException("Clock not initialized to a time taking mode!");
+ }
+}
+
+void Stopwatch::start(string perf_name)
+{
+ if (!active) return;
+
+ // Just works if not already present
+ records_of->insert(make_pair(perf_name, PerformanceData()));
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ // Take ctime
+ perf_info.clock_start = take_time();
+
+ // If this is a new start (i.e. not a restart)
+// if (!perf_info.paused)
+// perf_info.last_time = 0;
+
+ perf_info.paused = false;
+}
+
+void Stopwatch::stop(string perf_name)
+{
+ if (!active) return;
+
+ long double clock_end = take_time();
+
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ perf_info.stops++;
+ long double lapse = clock_end - perf_info.clock_start;
+
+ if ( mode == CPU_TIME )
+ lapse /= (double) CLOCKS_PER_SEC;
+
+ // Update last time
+ perf_info.last_time = lapse;
+
+ // Update min/max time
+ if ( lapse >= perf_info.max_time ) perf_info.max_time = lapse;
+ if ( lapse <= perf_info.min_time || perf_info.min_time == 0 )
+ perf_info.min_time = lapse;
+
+ // Update total time
+ perf_info.total_time += lapse;
+}
+
+void Stopwatch::pause(string perf_name)
+{
+ if (!active) return;
+
+ long double clock_end = clock();
+
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ long double lapse = clock_end - perf_info.clock_start;
+
+ // Update total time
+ perf_info.last_time += lapse;
+ perf_info.total_time += lapse;
+}
+
+void Stopwatch::reset_all()
+{
+ if (!active) return;
+
+ map<string, PerformanceData>::iterator it;
+
+ for (it = records_of->begin(); it != records_of->end(); ++it) {
+ reset(it->first);
+ }
+}
+
+void Stopwatch::report_all(int precision, std::ostream& output)
+{
+ if (!active) return;
+
+ output<< "\n*** PROFILING RESULTS [ms] (min - avg - max - lastTime - nSamples) ***\n";
+ map<string, PerformanceData>::iterator it;
+ for (it = records_of->begin(); it != records_of->end(); ++it) {
+ report(it->first, precision, output);
+ }
+}
+
+void Stopwatch::reset(string perf_name)
+{
+ if (!active) return;
+
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ perf_info.clock_start = 0;
+ perf_info.total_time = 0;
+ perf_info.min_time = 0;
+ perf_info.max_time = 0;
+ perf_info.last_time = 0;
+ perf_info.paused = false;
+ perf_info.stops = 0;
+}
+
+void Stopwatch::turn_on()
+{
+ std::cout << "Stopwatch active." << std::endl;
+ active = true;
+}
+
+void Stopwatch::turn_off()
+{
+ std::cout << "Stopwatch inactive." << std::endl;
+ active = false;
+}
+
+void Stopwatch::report(string perf_name, int precision, std::ostream& output)
+{
+ if (!active) return;
+
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ const int MAX_NAME_LENGTH = 40;
+ string pad = "";
+ for (int i = perf_name.length(); i<MAX_NAME_LENGTH; i++)
+ pad.append(" ");
+
+ output << perf_name << pad;
+ output << std::fixed << std::setprecision(precision)
+ << (perf_info.min_time*1e3) << "\t";
+ output << std::fixed << std::setprecision(precision)
+ << (perf_info.total_time*1e3 / (long double) perf_info.stops) << "\t";
+ output << std::fixed << std::setprecision(precision)
+ << (perf_info.max_time*1e3) << "\t";
+ output << std::fixed << std::setprecision(precision)
+ << (perf_info.last_time*1e3) << "\t";
+ output << std::fixed << std::setprecision(precision)
+ << perf_info.stops << std::endl;
+
+ // ostringstream stops;
+ // stops << perf_info.stops;
+ // output << " * Stops " << stops.str() << std::endl;
+ // output << std::endl;
+}
+
+long double Stopwatch::get_time_so_far(string perf_name)
+{
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ long double lapse =
+ (take_time() - (records_of->find(perf_name)->second).clock_start);
+
+ if (mode == CPU_TIME)
+ lapse /= (double) CLOCKS_PER_SEC;
+
+ return lapse;
+}
+
+long double Stopwatch::get_total_time(string perf_name)
+{
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ return perf_info.total_time;
+
+}
+
+long double Stopwatch::get_average_time(string perf_name)
+{
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ return (perf_info.total_time / (long double)perf_info.stops);
+
+}
+
+long double Stopwatch::get_min_time(string perf_name)
+{
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ return perf_info.min_time;
+
+}
+
+long double Stopwatch::get_max_time(string perf_name)
+{
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ return perf_info.max_time;
+
+}
+
+long double Stopwatch::get_last_time(string perf_name)
+{
+ // Try to recover performance data
+ if ( !performance_exists(perf_name) )
+ throw StopwatchException("Performance not initialized.");
+
+ PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ return perf_info.last_time;
+}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c65796713ddf422483f6c153af5e8ffc2cd4baae..d597f0f6ce8a818a5e9ad5ecd5f1813a6b86bbdc 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -5,6 +5,7 @@ include_directories("${INCLUDE_DIR}")
include_directories("${EIGEN3_INCLUDE_DIR}")
include_directories("${CDD_INCLUDE_DIR}")
include_directories("${CLP_INCLUDE_DIR}")
+include_directories("${qpOASES_INCLUDE_DIR}")
PROJECT(robust-equilibrium-lib)
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index 567c464b125c374ac5d53c6fe499dbe6d289f022..5f4bd9e2e856345f1e62d4ae2a881d75c378d691 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -8,13 +8,17 @@
#include "CoinBuild.hpp"
#include "CoinModel.hpp"
+#include <qpOASES.hpp>
+
#include <robust-equilibrium-lib/solver_LP_clp.hh>
+#include <robust-equilibrium-lib/solver_LP_qpoases.hh>
#include <iostream>
#include <iomanip>
using namespace std;
using namespace robust_equilibrium;
+USING_NAMESPACE_QPOASES
/** Example addRows.cpp */
void test_addRows()
@@ -365,6 +369,35 @@ int main()
{
cout <<"Test LP Solvers\n";
+ {
+ /* Setup data of first LP. */
+ real_t A[1*2] = { 1.0, 1.0 };
+ real_t g[2] = { 1.5, 1.0 };
+ real_t lb[2] = { 0.5, -2.0 };
+ real_t ub[2] = { 5.0, 2.0 };
+ real_t lbA[1] = { -1.0 };
+ real_t ubA[1] = { 2.0 };
+
+ /* Setup data of second LP. */
+ real_t g_new[2] = { 1.0, 1.5 };
+ real_t lb_new[2] = { 0.0, -1.0 };
+ real_t ub_new[2] = { 5.0, -0.5 };
+ real_t lbA_new[1] = { -2.0 };
+ real_t ubA_new[1] = { 1.0 };
+
+
+ /* Setting up QProblem object with zero Hessian matrix. */
+ QProblem example( 2,1,HST_ZERO );
+
+ Options options;
+ //options.setToMPC();
+ example.setOptions( options );
+
+ /* Solve first LP. */
+ int nWSR = 10;
+ example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
+ }
+
// test_addRows();
test_small_LP();
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index ee02e8ccf14cea899c84aeac36858f7b0691d6cd..c36cd787f39a2c52f3e9834d7972b84797b6e6f1 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -7,12 +7,20 @@
#include <iostream>
#include <robust-equilibrium-lib/static_equilibrium.hh>
#include <robust-equilibrium-lib/logger.hh>
+#include <robust-equilibrium-lib/stop-watch.hh>
using namespace robust_equilibrium;
using namespace Eigen;
using namespace std;
-
+#define PERF_PP "PP"
+#define PERF_LP_PREPARATION "LP preparation"
+#define PERF_LP_COIN "LP coin"
+#define PERF_LP_OASES "LP oases"
+#define PERF_LP2_COIN "LP2 coin"
+#define PERF_LP2_OASES "LP2 oases"
+#define PERF_DLP_COIN "DLP coin"
+#define PERF_DLP_OASES "DLP oases"
int main()
{
@@ -30,17 +38,22 @@ int main()
double LY = 0.5*0.138; // half foot size in y direction
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
- CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.0;
+ CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.5;
double gamma = atan(mu); // half friction cone angle
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
- RPY_LOWER_BOUNDS << -0.0*gamma, -0.0*gamma, -M_PI;
- RPY_UPPER_BOUNDS << +0.0*gamma, +0.0*gamma, +M_PI;
+ RPY_LOWER_BOUNDS << -0*gamma, -0*gamma, -M_PI;
+ RPY_UPPER_BOUNDS << +0*gamma, +0*gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
- const int GRID_SIZE = 20;
+ const int GRID_SIZE = 15;
StaticEquilibrium solver_PP(mass, generatorsPerContact, SOLVER_LP_CLP);
- StaticEquilibrium solver_LP(mass, generatorsPerContact, SOLVER_LP_CLP);
+ StaticEquilibrium solver_LP_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
+ StaticEquilibrium solver_LP_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
+ StaticEquilibrium solver_LP2_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
+ StaticEquilibrium solver_LP2_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
+ StaticEquilibrium solver_DLP_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
+ StaticEquilibrium solver_DLP_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
MatrixXX p = MatrixXX::Zero(4*N_CONTACTS,3); // contact points
@@ -70,7 +83,7 @@ int main()
break;
}
- // generate contact orientation
+// generate contact orientation
uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i),
@@ -86,13 +99,53 @@ int main()
printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
}
- if(!solver_LP.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
+ getProfiler().start(PERF_LP_PREPARATION);
+ if(!solver_LP_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
+ {
+ printf("Error while setting new contacts");
+ return -1;
+ }
+ getProfiler().stop(PERF_LP_PREPARATION);
+ getProfiler().start(PERF_LP_PREPARATION);
+ if(!solver_LP_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
+ {
+ printf("Error while setting new contacts");
+ return -1;
+ }
+ getProfiler().stop(PERF_LP_PREPARATION);
+ getProfiler().start(PERF_LP_PREPARATION);
+ if(!solver_LP2_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP2))
{
printf("Error while setting new contacts");
return -1;
}
+ getProfiler().stop(PERF_LP_PREPARATION);
+ getProfiler().start(PERF_LP_PREPARATION);
+ if(!solver_LP2_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP2))
+ {
+ printf("Error while setting new contacts");
+ return -1;
+ }
+ getProfiler().stop(PERF_LP_PREPARATION);
+ getProfiler().start(PERF_LP_PREPARATION);
+ if(!solver_DLP_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_DLP))
+ {
+ printf("Error while setting new contacts");
+ return -1;
+ }
+ getProfiler().stop(PERF_LP_PREPARATION);
+ getProfiler().start(PERF_LP_PREPARATION);
+ if(!solver_DLP_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_DLP))
+ {
+ printf("Error while setting new contacts");
+ return -1;
+ }
+ getProfiler().stop(PERF_LP_PREPARATION);
- if(!solver_PP.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP))
+ getProfiler().start(PERF_PP);
+ bool res = solver_PP.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP);
+ getProfiler().stop(PERF_PP);
+ if(!res)
{
printf("Error while setting new contacts");
return -1;
@@ -121,21 +174,81 @@ int main()
{
// uniform(com_LB, com_UB, com);
com(0) = x_range(j);
- double rob = solver_LP.computeEquilibriumRobustness(com);
+
+ getProfiler().start(PERF_LP_COIN);
+ double rob_coin = solver_LP_coin.computeEquilibriumRobustness(com);
+ getProfiler().stop(PERF_LP_COIN);
+
+ getProfiler().start(PERF_LP_OASES);
+ double rob_oases = solver_LP_oases.computeEquilibriumRobustness(com);
+ getProfiler().stop(PERF_LP_OASES);
+
+ getProfiler().start(PERF_LP2_COIN);
+ double rob_coin2 = solver_LP2_coin.computeEquilibriumRobustness(com);
+ getProfiler().stop(PERF_LP2_COIN);
+
+ getProfiler().start(PERF_LP2_OASES);
+ double rob_oases2 = solver_LP2_oases.computeEquilibriumRobustness(com);
+ getProfiler().stop(PERF_LP2_OASES);
+
+ getProfiler().start(PERF_DLP_COIN);
+ double rob_DLP_coin = solver_DLP_coin.computeEquilibriumRobustness(com);
+ getProfiler().stop(PERF_DLP_COIN);
+
+ getProfiler().start(PERF_DLP_OASES);
+ double rob_DLP_oases = solver_DLP_oases.computeEquilibriumRobustness(com);
+ getProfiler().stop(PERF_DLP_OASES);
+
+ if(fabs(rob_coin-rob_oases)>1e-5)
+ SEND_ERROR_MSG("LP_coin and LP_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_oases));
+
+ if(fabs(rob_coin-rob_oases2)>1e-5)
+ SEND_ERROR_MSG("LP_coin and LP2_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_oases2));
+
+// if(fabs(rob_coin-rob_coin2)>1e-5)
+// SEND_ERROR_MSG("LP_coin and LP2_coin returned different results: "+toString(rob_coin)+" VS "+toString(rob_coin2));
+
+ if(fabs(rob_coin-rob_DLP_oases)>1e-5)
+ SEND_ERROR_MSG("LP_coin and DLP_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_DLP_oases));
+
+ if(fabs(rob_coin-rob_DLP_coin)>1e-5)
+ SEND_ERROR_MSG("LP_coin and DLP_coin returned different results: "+toString(rob_coin)+" VS "+toString(rob_DLP_coin));
+
if(solver_PP.checkRobustEquilibrium(com, 0.0))
{
equilibrium(i,j) = 1.0;
- if(rob<=0)
- SEND_ERROR_MSG("PP says com is in equilibrium but LP find negative robustness "+toString(rob));
- if(rob>9.0)
- rob = 9.0;
- printf("%d ", (int)rob);
+ if(rob_coin<=0)
+ SEND_ERROR_MSG("PP says com is in equilibrium but LP_coin find negative robustness "+toString(rob_coin));
+ if(rob_oases<=0)
+ SEND_ERROR_MSG("PP says com is in equilibrium but LP_oases find negative robustness "+toString(rob_oases));
+ if(rob_coin2<=0)
+ SEND_ERROR_MSG("PP says com is in equilibrium but LP_coin2 find negative robustness "+toString(rob_coin2));
+ if(rob_oases2<=0)
+ SEND_ERROR_MSG("PP says com is in equilibrium but LP_oases2 find negative robustness "+toString(rob_oases2));
+ if(rob_DLP_oases<=0)
+ SEND_ERROR_MSG("PP says com is in equilibrium but DLP_oases find negative robustness "+toString(rob_DLP_oases));
+ if(rob_DLP_coin<=0)
+ SEND_ERROR_MSG("PP says com is in equilibrium but DLP_coin negative robustness "+toString(rob_DLP_coin));
+
+ if(rob_coin>9.0)
+ rob_coin = 9.0;
+ printf("%d ", (int)rob_coin);
}
else
{
equilibrium(i,j) = 0.0;
- if(rob>0)
- SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP find positive robustness "+toString(rob));
+ if(rob_coin>0)
+ SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_coin find positive robustness "+toString(rob_coin));
+ if(rob_oases>0)
+ SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_oases find positive robustness "+toString(rob_oases));
+ if(rob_coin2>0)
+ SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_coin2 find positive robustness "+toString(rob_coin2));
+ if(rob_oases2>0)
+ SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_oases2 find positive robustness "+toString(rob_oases2));
+ if(rob_DLP_coin>0)
+ SEND_ERROR_MSG("PP says com is NOT in equilibrium but DLP_coin find positive robustness "+toString(rob_DLP_coin));
+ if(rob_DLP_oases>0)
+ SEND_ERROR_MSG("PP says com is NOT in equilibrium but DLP_oases find positive robustness "+toString(rob_DLP_oases));
printf("- ");
}
@@ -154,7 +267,7 @@ int main()
printf("\n");
}
- cout<<"Max dist between contact points and grid points "<<minDistContactPoint.maxCoeff()<<"\n";
+// cout<<"Max dist between contact points and grid points "<<minDistContactPoint.maxCoeff()<<"\n";
cout<<"\nContact point position on the same grid\n";
bool contactPointDrawn;
@@ -178,5 +291,7 @@ int main()
printf("\n");
}
+ getProfiler().report_all();
+
return ret;
}