From 075b056c5c3b4b735c21330e468c02a956557d7b Mon Sep 17 00:00:00 2001
From: andreadelprete <andre.delprete@gmail.com>
Date: Mon, 26 Oct 2015 13:33:35 +0100
Subject: [PATCH] First (non-working) version of the library, containing just a
draft of the first test.
---
.gitmodules | 3 +
CMakeLists.txt | 53 +++++++
cmake | 1 +
cmake2/FindCDD.cmake | 25 ++++
cmake2/FindEigen2.cmake | 79 ++++++++++
cmake2/FindEigen3.cmake | 80 ++++++++++
include/robust-equilibrium-lib/config.hh | 113 ++++++++++++++
.../solver_LP_abstract.hh | 34 +++++
.../solver_LP_qpoases.hh | 0
.../static_equilibrium.hh | 43 ++++++
src/CMakeLists.txt | 31 ++++
src/solver_LP_qpoases.cpp | 0
src/static_equilibrium.cpp | 39 +++++
test/CMakeLists.txt | 19 +++
test/test_LP_solvers.cpp | 5 +
test/test_static_equilibrium.cpp | 140 ++++++++++++++++++
16 files changed, 665 insertions(+)
create mode 100644 .gitmodules
create mode 100644 CMakeLists.txt
create mode 160000 cmake
create mode 100644 cmake2/FindCDD.cmake
create mode 100644 cmake2/FindEigen2.cmake
create mode 100644 cmake2/FindEigen3.cmake
create mode 100644 include/robust-equilibrium-lib/config.hh
create mode 100644 include/robust-equilibrium-lib/solver_LP_abstract.hh
create mode 100644 include/robust-equilibrium-lib/solver_LP_qpoases.hh
create mode 100644 include/robust-equilibrium-lib/static_equilibrium.hh
create mode 100644 src/CMakeLists.txt
create mode 100644 src/solver_LP_qpoases.cpp
create mode 100644 src/static_equilibrium.cpp
create mode 100644 test/CMakeLists.txt
create mode 100644 test/test_LP_solvers.cpp
create mode 100644 test/test_static_equilibrium.cpp
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..2b7a4fe
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "cmake"]
+ path = cmake
+ url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..5faf2e4
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 2.6)
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/cpack.cmake)
+
+
+SET(PROJECT_NAME robust-equilibrium-lib)
+SET(PROJECT_DESCRIPTION
+ "Utility classes for testing (robust) static equilibrium of a system in contact with the environment"
+ )
+SET(PROJECT_URL "")
+
+set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/;${PROJECT_SOURCE_DIR}/cmake2/")
+set(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib/")
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin/")
+
+set(SRC_DIR "${PROJECT_SOURCE_DIR}/src")
+set(INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include")
+
+set(EXTERNAL_LIBRARY_DIR "${PROJECT_SOURCE_DIR}/external/lib")
+
+if ( MSVC )
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNOMINMAX")
+endif()
+
+SETUP_PROJECT()
+
+string (REPLACE "-Werror" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
+MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
+
+# Declare Headers
+SET(${PROJECT_NAME}_HEADERS
+ include/robust-equilibrium-lib/config.hh
+ include/robust-equilibrium-lib/solver_LP_abstract.hh
+ include/robust-equilibrium-lib/solver_LP_qpoases.hh
+ include/robust-equilibrium-lib/static_equilibrium.hh
+ )
+
+find_package(Eigen3)
+if (EIGEN3_INCLUDE_DIR)
+ message(STATUS "Found Eigen3")
+else()
+ message(STATUS "Eigen3 not found looking for Eigen2")
+ find_package(Eigen2 REQUIRED)
+endif()
+
+find_package(CDD REQUIRED)
+
+add_subdirectory (src)
+add_subdirectory (test)
+
+SETUP_PROJECT_FINALIZE()
+SETUP_PROJECT_CPACK()
+
diff --git a/cmake b/cmake
new file mode 160000
index 0000000..ad5a548
--- /dev/null
+++ b/cmake
@@ -0,0 +1 @@
+Subproject commit ad5a5486c4476e5e92cfd688b82467b54957a570
diff --git a/cmake2/FindCDD.cmake b/cmake2/FindCDD.cmake
new file mode 100644
index 0000000..54f859d
--- /dev/null
+++ b/cmake2/FindCDD.cmake
@@ -0,0 +1,25 @@
+# - Try to find libcdd
+# Once done this will define
+# CDD_FOUND - System has CDD
+# CDD_INCLUDE_DIRS - The CDD include directories
+# CDD_LIBRARIES - The libraries needed to use CDD
+# CDD_DEFINITIONS - Compiler switches required for using CDD
+
+
+find_path(CDD_INCLUDE_DIR cdd/cdd.h
+ HINTS ${CDD_INCLUDEDIR}
+ PATH_SUFFIXES CDD )
+
+find_library(CDD_LIBRARY NAMES libcdd
+ HINTS ${CDD_LIBDIR} ${CDD_LIBRARY_DIRS} )
+
+set(CDD_LIBRARIES ${CDD_LIBRARY} )
+set(CDD_INCLUDE_DIRS ${CDD_INCLUDE_DIR} )
+
+include(FindPackageHandleStandardArgs)
+# handle the QUIETLY and REQUIRED arguments and set CDD_FOUND to TRUE
+# if all listed variables are TRUE
+find_package_handle_standard_args(CDD DEFAULT_MSG
+ CDD_LIBRARY CDD_INCLUDE_DIR)
+
+mark_as_advanced(CDD_INCLUDE_DIR CDD_LIBRARY )
diff --git a/cmake2/FindEigen2.cmake b/cmake2/FindEigen2.cmake
new file mode 100644
index 0000000..7f0bbed
--- /dev/null
+++ b/cmake2/FindEigen2.cmake
@@ -0,0 +1,79 @@
+# - Try to find Eigen2 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+# find_package(Eigen2 2.0.3)
+# to require version 2.0.3 to newer of Eigen2.
+#
+# Once done this will define
+#
+# EIGEN2_FOUND - system has eigen lib with correct version
+# EIGEN2_INCLUDE_DIR - the eigen include directory
+# EIGEN2_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Redistribution and use is allowed according to the terms of the BSD license.
+
+if(NOT Eigen2_FIND_VERSION)
+ if(NOT Eigen2_FIND_VERSION_MAJOR)
+ set(Eigen2_FIND_VERSION_MAJOR 2)
+ endif(NOT Eigen2_FIND_VERSION_MAJOR)
+ if(NOT Eigen2_FIND_VERSION_MINOR)
+ set(Eigen2_FIND_VERSION_MINOR 0)
+ endif(NOT Eigen2_FIND_VERSION_MINOR)
+ if(NOT Eigen2_FIND_VERSION_PATCH)
+ set(Eigen2_FIND_VERSION_PATCH 0)
+ endif(NOT Eigen2_FIND_VERSION_PATCH)
+
+ set(Eigen2_FIND_VERSION "${Eigen2_FIND_VERSION_MAJOR}.${Eigen2_FIND_VERSION_MINOR}.${Eigen2_FIND_VERSION_PATCH}")
+endif(NOT Eigen2_FIND_VERSION)
+
+macro(_eigen2_check_version)
+ file(READ "${EIGEN2_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen2_version_header)
+
+ string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen2_world_version_match "${_eigen2_version_header}")
+ set(EIGEN2_WORLD_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen2_major_version_match "${_eigen2_version_header}")
+ set(EIGEN2_MAJOR_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen2_minor_version_match "${_eigen2_version_header}")
+ set(EIGEN2_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ set(EIGEN2_VERSION ${EIGEN2_WORLD_VERSION}.${EIGEN2_MAJOR_VERSION}.${EIGEN2_MINOR_VERSION})
+ if((${EIGEN2_WORLD_VERSION} NOTEQUAL 2) OR (${EIGEN2_MAJOR_VERSION} GREATER 10) OR (${EIGEN2_VERSION} VERSION_LESS ${Eigen2_FIND_VERSION}))
+ set(EIGEN2_VERSION_OK FALSE)
+ else()
+ set(EIGEN2_VERSION_OK TRUE)
+ endif()
+
+ if(NOT EIGEN2_VERSION_OK)
+
+ message(STATUS "Eigen2 version ${EIGEN2_VERSION} found in ${EIGEN2_INCLUDE_DIR}, "
+ "but at least version ${Eigen2_FIND_VERSION} is required")
+ endif(NOT EIGEN2_VERSION_OK)
+endmacro(_eigen2_check_version)
+
+if (EIGEN2_INCLUDE_DIR)
+
+ # in cache already
+ _eigen2_check_version()
+ set(EIGEN2_FOUND ${EIGEN2_VERSION_OK})
+
+else (EIGEN2_INCLUDE_DIR)
+
+find_path(EIGEN2_INCLUDE_DIR NAMES Eigen/Core
+ PATHS
+ ${INCLUDE_INSTALL_DIR}
+ ${KDE4_INCLUDE_DIR}
+ PATH_SUFFIXES eigen2
+ )
+
+if(EIGEN2_INCLUDE_DIR)
+ _eigen2_check_version()
+endif(EIGEN2_INCLUDE_DIR)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Eigen2 DEFAULT_MSG EIGEN2_INCLUDE_DIR EIGEN2_VERSION_OK)
+
+mark_as_advanced(EIGEN2_INCLUDE_DIR)
+
+endif(EIGEN2_INCLUDE_DIR)
diff --git a/cmake2/FindEigen3.cmake b/cmake2/FindEigen3.cmake
new file mode 100644
index 0000000..66ffe8e
--- /dev/null
+++ b/cmake2/FindEigen3.cmake
@@ -0,0 +1,80 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+# find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+# EIGEN3_FOUND - system has eigen lib with correct version
+# EIGEN3_INCLUDE_DIR - the eigen include directory
+# EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+ if(NOT Eigen3_FIND_VERSION_MAJOR)
+ set(Eigen3_FIND_VERSION_MAJOR 2)
+ endif(NOT Eigen3_FIND_VERSION_MAJOR)
+ if(NOT Eigen3_FIND_VERSION_MINOR)
+ set(Eigen3_FIND_VERSION_MINOR 91)
+ endif(NOT Eigen3_FIND_VERSION_MINOR)
+ if(NOT Eigen3_FIND_VERSION_PATCH)
+ set(Eigen3_FIND_VERSION_PATCH 0)
+ endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+ set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+ file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+ string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+ set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+ if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK FALSE)
+ else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK TRUE)
+ endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+ if(NOT EIGEN3_VERSION_OK)
+
+ message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+ "but at least version ${Eigen3_FIND_VERSION} is required")
+ endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+ # in cache already
+ _eigen3_check_version()
+ set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+ find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+ PATHS
+ ${CMAKE_INSTALL_PREFIX}/include
+ ${KDE4_INCLUDE_DIR}
+ PATH_SUFFIXES eigen3 eigen
+ )
+
+ if(EIGEN3_INCLUDE_DIR)
+ _eigen3_check_version()
+ endif(EIGEN3_INCLUDE_DIR)
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+ mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
diff --git a/include/robust-equilibrium-lib/config.hh b/include/robust-equilibrium-lib/config.hh
new file mode 100644
index 0000000..ef304c4
--- /dev/null
+++ b/include/robust-equilibrium-lib/config.hh
@@ -0,0 +1,113 @@
+#ifndef _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
+#define _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
+
+#include <Eigen/Dense>
+#include <Eigen/src/Core/util/Macros.h>
+
+// Package version (header).
+# define ROBUST_EQUILIBRIUM_VERSION "UNKNOWN"
+
+// Handle portable symbol export.
+// Defining manually which symbol should be exported is required
+// under Windows whether MinGW or MSVC is used.
+//
+// The headers then have to be able to work in two different modes:
+// - dllexport when one is building the library,
+// - dllimport for clients using the library.
+//
+// On Linux, set the visibility accordingly. If C++ symbol visibility
+// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
+# if defined _WIN32 || defined __CYGWIN__
+// On Microsoft Windows, use dllimport and dllexport to tag symbols.
+# define ROBUST_EQUILIBRIUM_DLLIMPORT __declspec(dllimport)
+# define ROBUST_EQUILIBRIUM_DLLEXPORT __declspec(dllexport)
+# define ROBUST_EQUILIBRIUM_DLLLOCAL
+# else
+// On Linux, for GCC >= 4, tag symbols using GCC extension.
+# if __GNUC__ >= 4
+# define ROBUST_EQUILIBRIUM_DLLIMPORT __attribute__ ((visibility("default")))
+# define ROBUST_EQUILIBRIUM_DLLEXPORT __attribute__ ((visibility("default")))
+# define ROBUST_EQUILIBRIUM_DLLLOCAL __attribute__ ((visibility("hidden")))
+# else
+// Otherwise (GCC < 4 or another compiler is used), export everything.
+# define ROBUST_EQUILIBRIUM_DLLIMPORT
+# define ROBUST_EQUILIBRIUM_DLLEXPORT
+# define ROBUST_EQUILIBRIUM_DLLLOCAL
+# endif // __GNUC__ >= 4
+# endif // defined _WIN32 || defined __CYGWIN__
+
+# ifdef ROBUST_EQUILIBRIUM_STATIC
+// If one is using the library statically, get rid of
+// extra information.
+# define ROBUST_EQUILIBRIUM_DLLAPI
+# define ROBUST_EQUILIBRIUM_LOCAL
+# else
+// Depending on whether one is building or using the
+// library define DLLAPI to import or export.
+# ifdef ROBUST_EQUILIBRIUM_EXPORTS
+# define ROBUST_EQUILIBRIUM_DLLAPI ROBUST_EQUILIBRIUM_DLLEXPORT
+# else
+# define ROBUST_EQUILIBRIUM_DLLAPI ROBUST_EQUILIBRIUM_DLLIMPORT
+# endif // ROBUST_EQUILIBRIUM_EXPORTS
+# define ROBUST_EQUILIBRIUM_LOCAL ROBUST_EQUILIBRIUM_DLLLOCAL
+# endif // ROBUST_EQUILIBRIUM_STATIC
+
+namespace robust_equilibrium
+{
+
+//#define USE_FLOAT 1;
+#ifdef USE_FLOAT
+typedef float value_type;
+#else
+typedef double value_type;
+#endif
+
+typedef Eigen::Matrix <value_type, 2, 1> Vector2;
+typedef Eigen::Matrix <value_type, 1, 2> RVector2;
+typedef Eigen::Matrix <value_type, 3, 1> Vector3;
+typedef Eigen::Matrix <value_type, 1, 3> RVector3;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> VectorX;
+typedef Eigen::Matrix <value_type, 1, Eigen::Dynamic> RVectorX;
+typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Rotation;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
+typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
+
+//define Eigen ref if available
+#if EIGEN_VERSION_AT_LEAST(3,2,0)
+typedef Eigen::Ref<Vector2> Ref_vector2;
+typedef Eigen::Ref<Vector3> Ref_vector3;
+typedef Eigen::Ref<VectorX> Ref_vectorX;
+typedef Eigen::Ref<Rotation> Ref_rotation;
+typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
+typedef Eigen::Ref<Matrix43> Ref_matrix43;
+typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
+
+typedef const Eigen::Ref<const Vector2> & Cref_vector2;
+typedef const Eigen::Ref<const Vector3> & Cref_vector3;
+typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
+typedef const Eigen::Ref<const Rotation> & Cref_rotation;
+typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
+typedef const Eigen::Ref<const Matrix43> & Cref_matrix43;
+typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
+#else
+typedef vector2_t & Ref_vector2;
+typedef vector3_t & Ref_vector3;
+typedef vector_t & Ref_vector;
+typedef rotation_t & Ref_rotation;
+typedef T_rotation_t& Ref_matrixX3;
+typedef T_rotation_t& Ref_matrix43;
+typedef matrix_t & Ref_matrixXX;
+
+typedef const vector2_t & Cref_vector2;
+typedef const vector3_t & Cref_vector3;
+typedef const vector_t & Cref_vector;
+typedef const rotation_t & Cref_rotation;
+typedef const T_rotation_t& Cref_matrixX3;
+typedef const T_rotation_t& Cref_matrix43;
+typedef const matrix_t & Cref_matrixXX;
+#endif
+
+} //namespace robust_equilibrium
+
+#endif //_ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
diff --git a/include/robust-equilibrium-lib/solver_LP_abstract.hh b/include/robust-equilibrium-lib/solver_LP_abstract.hh
new file mode 100644
index 0000000..41ad038
--- /dev/null
+++ b/include/robust-equilibrium-lib/solver_LP_abstract.hh
@@ -0,0 +1,34 @@
+#ifndef ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_ABSTRACT_HH
+#define ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_ABSTRACT_HH
+
+#include <Eigen/Dense>
+#include <robust-equilibrium-lib/config.hh>
+
+namespace robust_equilibrium
+{
+
+enum ROBUST_EQUILIBRIUM_DLLAPI SolverLP
+{
+ SOLVER_LP_QPOASES
+};
+
+class ROBUST_EQUILIBRIUM_DLLAPI SolverLPAbstract
+{
+private:
+ std::string m_name;
+
+public:
+
+ SolverLPAbstract(std::string name);
+
+ /** Solve the linear program
+ * minimize c' x
+ * subject to lb <= A x <= ub
+ */
+ virtual bool solve(Cref_vectorX c, Cref_matrixXX A, Cref_vectorX lb, Cref_vectorX ub) = 0;
+
+};
+
+} // end namespace robust_equilibrium
+
+#endif //ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_ABSTRACT_HH
diff --git a/include/robust-equilibrium-lib/solver_LP_qpoases.hh b/include/robust-equilibrium-lib/solver_LP_qpoases.hh
new file mode 100644
index 0000000..e69de29
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
new file mode 100644
index 0000000..3fcc887
--- /dev/null
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -0,0 +1,43 @@
+#ifndef ROBUST_EQUILIBRIUM_LIB_STATIC_EQUILIBRIUM_H
+#define ROBUST_EQUILIBRIUM_LIB_STATIC_EQUILIBRIUM_H
+
+#include <Eigen/Dense>
+#include <robust-equilibrium-lib/config.hh>
+#include <robust-equilibrium-lib/solver_LP_abstract.hh>
+
+namespace robust_equilibrium
+{
+
+enum ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibriumAlgorithm
+{
+ STATIC_EQUILIBRIUM_ALGORITHM_LP,
+ STATIC_EQUILIBRIUM_ALGORITHM_PP,
+ STATIC_EQUILIBRIUM_ALGORITHM_IP,
+ STATIC_EQUILIBRIUM_ALGORITHM_DIP
+};
+
+class ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibrium
+{
+private:
+ StaticEquilibriumAlgorithm m_algorithm;
+ SolverLPAbstract* m_solver;
+
+public:
+ StaticEquilibrium(unsigned int generatorsPerContact, SolverLP solver_type);
+
+ bool setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
+ Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg);
+
+ double computeEquilibriumRobustness(Cref_vector2 com);
+
+ double checkRobustEquilibrium(Cref_vector2 com, double e_max=0.0);
+
+ double findExtremumOverLine(Cref_vector2 a, double b, double e_max=0.0);
+
+ double findExtremumInDirection(Cref_vector2 direction, double e_max=0.0);
+
+};
+
+} // end namespace robust_equilibrium
+
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000..3ff2216
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.6)
+
+include_directories("${SRC_DIR}")
+include_directories("${INCLUDE_DIR}")
+include_directories("${EIGEN3_INCLUDE_DIR}")
+include_directories("${CDD_INCLUDE_DIR}")
+
+SET(LIBRARY_NAME ${PROJECT_NAME})
+
+SET(${LIBRARY_NAME}_SOURCES
+ ${INCLUDE_DIR}/robust-equilibrium-lib/config.hh
+ ${INCLUDE_DIR}/robust-equilibrium-lib/solver_LP_abstract.hh
+ ${INCLUDE_DIR}/robust-equilibrium-lib/solver_LP_qpoases.hh
+ ${INCLUDE_DIR}/robust-equilibrium-lib/static_equilibrium.hh
+ static_equilibrium.cpp
+ solver_LP_qpoases.cpp
+ )
+
+INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
+ADD_LIBRARY(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES})
+
+if ( MSVC )
+ SET(CMAKE_DEBUG_POSTFIX d)
+endif ( MSVC )
+
+TARGET_LINK_LIBRARIES(robust-equilibrium-lib ${CDD_LIBRARY})
+
+SET_TARGET_PROPERTIES(robust-equilibrium-lib PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}")
+SET_TARGET_PROPERTIES(robust-equilibrium-lib PROPERTIES ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}")
+
+INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
diff --git a/src/solver_LP_qpoases.cpp b/src/solver_LP_qpoases.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
new file mode 100644
index 0000000..a136d38
--- /dev/null
+++ b/src/static_equilibrium.cpp
@@ -0,0 +1,39 @@
+#include <robust-equilibrium-lib/static_equilibrium.hh>
+
+namespace robust_equilibrium
+{
+
+
+StaticEquilibrium::StaticEquilibrium(unsigned int generatorsPerContact, SolverLP solver_type)
+{
+
+}
+
+bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
+ Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg)
+{
+ return true;
+}
+
+double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
+{
+ return 0.0;
+}
+
+double StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
+{
+ return 0.0;
+}
+
+double StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, double b, double e_max)
+{
+ return 0.0;
+}
+
+double StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, double e_max)
+{
+ return 0.0;
+}
+
+
+} // end namespace robust_equilibrium
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
new file mode 100644
index 0000000..d52109d
--- /dev/null
+++ b/test/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 2.6)
+
+include_directories("${SRC_DIR}")
+include_directories("${INCLUDE_DIR}")
+include_directories("${EIGEN3_INCLUDE_DIR}")
+include_directories("${CDD_INCLUDE_DIR}")
+
+PROJECT(robust-equilibrium-lib)
+
+if ( MSVC )
+ SET(CMAKE_DEBUG_POSTFIX d)
+endif ( MSVC )
+
+add_executable(test_static_equilibrium test_static_equilibrium.cpp)
+add_executable(test_LP_solvers test_LP_solvers.cpp)
+
+TARGET_LINK_LIBRARIES(test_LP_solvers robust-equilibrium-lib)
+TARGET_LINK_LIBRARIES(test_static_equilibrium robust-equilibrium-lib)
+#~ TARGET_LINK_LIBRARIES(polytopetest polytope ${SRC_DIR}/../external/cddlib-094b/lib-src/libcdd.a)
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
new file mode 100644
index 0000000..d56f6b7
--- /dev/null
+++ b/test/test_LP_solvers.cpp
@@ -0,0 +1,5 @@
+int main()
+{
+ int ret = 0;
+ return ret;
+}
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
new file mode 100644
index 0000000..71642fc
--- /dev/null
+++ b/test/test_static_equilibrium.cpp
@@ -0,0 +1,140 @@
+#include <vector>
+#include <Eigen/Dense>
+#include <Eigen/src/Core/util/Macros.h>
+
+#include "cdd/cddmp.h"
+#include "cdd/setoper.h"
+#include "cdd/cddtypes.h"
+#include "cdd/cdd.h"
+
+#include <robust-equilibrium-lib/static_equilibrium.hh>
+
+using namespace robust_equilibrium;
+using namespace Eigen;
+
+typedef Eigen::AngleAxis<value_type> angle_axis_t;
+
+
+void init_library()
+{
+ dd_set_global_constants();dd_debug = false;
+}
+
+void release_library()
+{
+ //dd_free_global_constants();
+}
+
+bool uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out)
+{
+
+ assert(lower_bounds.rows()==out.rows());
+ assert(upper_bounds.rows()==out.rows());
+ assert(lower_bounds.cols()==out.cols());
+ assert(upper_bounds.cols()==out.cols());
+ for(int i=0; i<out.rows(); i++)
+ for(int j=0; j<out.cols(); j++)
+ out(i,j) = (rand()/ value_type(RAND_MAX))*(upper_bounds(i,j)-lower_bounds(i,j)) + lower_bounds(i,j);
+}
+
+bool euler_matrix(double roll, double pitch, double yaw, Ref_rotation R)
+{
+ R = (angle_axis_t(roll, Vector3::UnitX())
+ * angle_axis_t(pitch, Vector3::UnitY())
+ * angle_axis_t(yaw, Vector3::UnitZ())).toRotationMatrix();
+}
+
+bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
+ Ref_matrix43 p, Ref_matrix43 N)
+{
+ // compute rotation matrix
+ Quaternion<value_type> q = angle_axis_t(rpy(0), Vector3::UnitX())
+ * angle_axis_t(rpy(1), Vector3::UnitY())
+ * angle_axis_t(rpy(2), Vector3::UnitZ());
+ //euler_matrix(rpy[0], rpy[1], rpy[2], R);
+ // contact points in local frame
+ p << lx, ly, 0,
+ lx, -ly, 0,
+ -lx, -ly, 0,
+ -lx, ly, 0;
+ // contact points in world frame
+ p.row(0) = pos + q*p.row(0);
+ p.row(1) = pos + q*p.row(1);
+ p.row(2) = pos + q*p.row(2);
+ p.row(3) = pos + q*p.row(3);
+ // normal direction in local frame
+ RVector3 n;
+ n << 0, 0, 1;
+ // normal directions in world frame
+ n = q*n;
+ N << n, n, n, n;
+ return true;
+}
+
+int main()
+{
+ int ret = 0;
+ double mu = 0.3; // friction coefficient
+ unsigned int generatorsPerContact = 4;
+ unsigned int N_CONTACTS = 8;
+ unsigned int N_POINTS = 50; // number of com positions to test
+ double MIN_FEET_DISTANCE = 0.3;
+ double LX = 0.5*0.2172; // half foot size in x direction
+ 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.5, -0.5, 0.0;
+ CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 1.0;
+ double gamma = atan(mu); // half friction cone angle
+ RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
+ RPY_LOWER_BOUNDS << -2*gamma, -2*gamma, -M_PI;
+ RPY_UPPER_BOUNDS << +2*gamma, +2*gamma, +M_PI;
+ double X_MARG = 0.07;
+ double Y_MARG = 0.07;
+
+ StaticEquilibrium solver(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
+ MatrixXX N = MatrixXX::Zero(4*N_CONTACTS,3); // contact normals
+ VectorX frictionCoefficients(N_CONTACTS);
+ frictionCoefficients.fill(mu);
+
+ // Generate contact positions and orientations
+ bool collision;
+ for(int i=0; i<N_CONTACTS; i++)
+ {
+ while(true) // generate contact position
+ {
+ uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
+ collision = false;
+ for(int j=0; j<i-1; j++)
+ if((contact_pos.row(i)-contact_pos.row(j)).norm() < MIN_FEET_DISTANCE)
+ collision = true;
+ if(collision==false)
+ break;
+ }
+
+ // 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),
+ p.middleRows<4>(i*4), N.middleRows<4>(i*4));
+ }
+
+ solver.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP);
+
+
+ RVector2 com_LB, com_UB;
+ com_LB(0) = p.col(0).minCoeff()-X_MARG;
+ com_UB(0) = p.col(0).maxCoeff()+X_MARG;
+ com_LB(1) = p.col(1).minCoeff()-Y_MARG;
+ com_UB(1) = p.col(1).maxCoeff()+Y_MARG;
+ MatrixXX com(N_POINTS,2);
+ for(int i=0; i<N_POINTS; i++)
+ {
+ uniform(com_LB, com_UB, com.row(i));
+ double robustness = solver.checkRobustEquilibrium(com.row(i), 0.0);
+ printf("CoM position (%.3f, %.3f) robustness %.3f\n", com(i,0), com(i,1), robustness);
+ }
+
+ return ret;
+}
--
GitLab