Unverified Commit e05f1e4d authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #2 from pFernbach/dynamicEquilibrium

Dynamic equilibrium
parents 50e40834 2cbd3cdd
......@@ -11,7 +11,7 @@ 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(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/build/")
set(SRC_DIR "${PROJECT_SOURCE_DIR}/src")
set(INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include")
......
......@@ -119,6 +119,8 @@ public:
EquilibriumAlgorithm getAlgorithm(){ return m_algorithm; }
void setAlgorithm(EquilibriumAlgorithm algorithm);
/**
* @brief Specify a new set of contacts.
* All 3d vectors are expressed in a reference frame having the z axis aligned with gravity.
......@@ -147,6 +149,8 @@ public:
bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg);
void setG(Cref_matrix6X G){m_G_centr = G;}
/**
* @brief Compute a measure of the robustness of the equilibrium of the specified com position.
* This amounts to solving the following LP:
......@@ -280,7 +284,7 @@ public:
* @brief findMaximumAcceleration Find the maximal acceleration along a given direction
find b, alpha0
maximize alpha0
subject to -h <= [-G (Hv)] [b a0]^T <= -h
subject to [-G (Hv)] [b a0]^T = -h
0 <= [b a0]^T <= Inf
......@@ -289,13 +293,15 @@ public:
alpha0 is the maximal amplitude of the acceleration, for the given direction v
c is the CoM position
G is the matrix whose columns are the gravito-inertial wrench generators
A is [-G (Hv)]
* @param A
* @param h
* @param alpha0
H is m*[I3 ĉ]^T
h is m*[-g (c x -g)]^T
* @param H input
* @param h input
* @param v input
* @param alpha0 output
* @return The status of the LP solver.
*/
LP_status findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0);
LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 v, double& alpha0);
/**
* @brief checkAdmissibleAcceleration return true if the given acceleration is admissible for the given contacts
......@@ -305,14 +311,12 @@ public:
b are the coefficient of the contact force generators (f = V b)
a is the vector3 defining the acceleration
G is the matrix whose columns are the gravito-inertial wrench generators
h and H come from polytope inequalities
* @param G
* @param H
* @param h
* @param a
H is m*[I3 ĉ]^T
h is m*[-g (c x -g)]^T
* @param a the acceleration
* @return true if the acceleration is admissible, false otherwise
*/
bool checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a );
bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a );
};
......
......@@ -62,6 +62,7 @@ namespace centroidal_dynamics
typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
typedef const Eigen::Ref<const Matrix43> & Cref_matrix43;
typedef const Eigen::Ref<const Matrix6X> & Cref_matrix6X;
typedef const Eigen::Ref<const Matrix63> & Cref_matrix63;
typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
/**Column major definitions for compatibility with classical eigen use**/
......
......@@ -30,6 +30,8 @@ boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self)
{
MatrixXX H;
VectorX h;
H = MatrixXX::Zero(6,6);
h = VectorX::Zero(6,1);
self.getPolytopeInequalities(H,h);
MatrixXXColMajor _H = H;
return boost::python::make_tuple(_H, h);
......
......@@ -130,6 +130,13 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
return true;
}
void Equilibrium::setAlgorithm(EquilibriumAlgorithm algorithm){
if(algorithm == EQUILIBRIUM_ALGORITHM_PP && m_G_centr.rows() > 0)
SEND_DEBUG_MSG("Cannot set algorithm to PP after setting contacts, you should call again setNewContact with PP algorithm");
else
m_algorithm = algorithm;
}
bool Equilibrium::setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
......@@ -324,12 +331,6 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
return LP_STATUS_ERROR;
}
/**
m_d.setZero();
m_d.head<3>() = m_mass*m_gravity;
m_D.setZero();
m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
*/
LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
{
......@@ -523,6 +524,9 @@ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vecto
bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
{
int n = (int)(v.rows());
int m = (int)(v.cols());
// getProfiler().start("eigen_to_cdd");
dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
// getProfiler().stop("eigen_to_cdd");
......@@ -577,6 +581,15 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
}
// getProfiler().stop("cdd to eigen");
std::cout<<" inequalities : m = "<<m<<std::endl;
if(m_h.rows() < n )
{
SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
m_is_cdd_stable = false;
return false;
}
return true;
}
......@@ -591,18 +604,28 @@ double Equilibrium::convert_emax_to_b0(double emax)
}
LP_status Equilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0){
int m = (int)A.cols() -1 ; // 4* number of contacts
VectorX b_a0(m+1);
VectorX c = VectorX::Zero(m+1);
c(m) = -1.0; // because we search max alpha0
VectorX lb = VectorX::Zero(m+1);
VectorX ub = VectorX::Ones(m+1)*1e10; // Inf
LP_status Equilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,Cref_vector3 v, double& alpha0){
int m = (int)m_G_centr.cols(); // 4* number of contacts
VectorX b_a0(m);
VectorX c = VectorX::Zero(m);
MatrixXX A = MatrixXX::Zero(6, m);
A.topLeftCorner(6,m) = - m_G_centr;
A.topRightCorner(6,1) = H * v;
c(m-1) = -1.0; // because we search max alpha0, and c = [ B alpha ]^t
VectorX lb = VectorX::Zero(m);
VectorX ub = VectorX::Ones(m)*1e10; // Inf
VectorX Alb = -h;
VectorX Aub = -h;
int iter = 0;
LP_status lpStatus;
do{
lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_a0);
iter ++;
}while(lpStatus == LP_STATUS_ERROR && iter < 3);
LP_status lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_a0);
if(lpStatus==LP_STATUS_UNBOUNDED){
//SEND_DEBUG_MSG("Primal LP problem is unbounded : "+toString(lpStatus));
alpha0 = std::numeric_limits<double>::infinity();
......@@ -619,8 +642,9 @@ LP_status Equilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h,
}
bool Equilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a ){
int m = (int)G.cols(); // number of contact * 4
bool Equilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a ){
int m = (int)m_G_centr.cols(); // number of contact * generator per contacts
VectorX b(m);
VectorX c = VectorX::Zero(m);
VectorX lb = VectorX::Zero(m);
......@@ -630,9 +654,9 @@ bool Equilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H,
int iter = 0;
LP_status lpStatus;
do{
lpStatus = m_solver->solve(c, lb, ub, G, Alb, Aub, b);
lpStatus = m_solver->solve(c, lb, ub, m_G_centr, Alb, Aub, b);
iter ++;
}while(lpStatus == LP_STATUS_ERROR && iter < 5);
}while(lpStatus == LP_STATUS_ERROR && iter < 3);
if(lpStatus==LP_STATUS_OPTIMAL || lpStatus==LP_STATUS_UNBOUNDED)
{
......
cmake_minimum_required(VERSION 2.6)
include_directories("${SRC_DIR}")
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
include_directories("${INCLUDE_DIR}")
include_directories("${EIGEN3_INCLUDE_DIR}")
include_directories("${CDD_INCLUDE_DIR}")
# no need to include directories for qpOASES as it is automatically done through pkgconfig
if(CLP_FOUND)
include_directories("${CLP_INCLUDE_DIR}")
endif()
PROJECT(centroidal-dynamics-lib)
# Make Boost.Test generates the main function in test cases.
#ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
# ADD_TESTCASE(NAME)
# ------------------------
#
# Define a test named `NAME'.
#
# This macro will create a binary from `NAME.cc', link it against
# Boost and add it to the test suite.
#
MACRO(ADD_TESTCASE NAME GENERATED)
IF (${GENERATED} STREQUAL TRUE)
ADD_EXECUTABLE(${NAME} ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.cpp)
ELSE()
ADD_EXECUTABLE(${NAME} ${NAME}.cpp)
ENDIF()
ADD_TEST(${NAME} ${RUNTIME_OUTPUT_DIRECTORY}/${NAME})
# Link against Boost and project library.
TARGET_LINK_LIBRARIES(${NAME}
${Boost_LIBRARIES}
${PROJECT_NAME}
)
ENDMACRO(ADD_TESTCASE)
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)
ADD_TESTCASE(test_static_equilibrium FALSE)
ADD_TESTCASE(test_LP_solvers FALSE)
TARGET_LINK_LIBRARIES(test_LP_solvers centroidal-dynamics-lib)
PKG_CONFIG_USE_DEPENDENCY(test_LP_solvers qpOASES)
TARGET_LINK_LIBRARIES(test_static_equilibrium centroidal-dynamics-lib)
#~ TARGET_LINK_LIBRARIES(polytopetest polytope ${SRC_DIR}/../external/cddlib-094b/lib-src/libcdd.a)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment