Unverified Commit facb7365 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #4 from stonneau/rbprm_v_1

Rbprm v 1
parents 8106fdc0 94614438
......@@ -31,7 +31,7 @@ SETUP_PROJECT()
string (REPLACE "-Werror" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" OFF)
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
# search for python
FINDPYTHON(2.7 EXACT REQUIRED)
......
......@@ -29,6 +29,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium
public:
const double m_mass; /// mass of the system
const Vector3 m_gravity; /// gravity vector
/** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
Matrix6X m_G_centr;
private:
static bool m_is_cdd_initialized; /// true if cdd lib has been initialized, false otherwise
......@@ -40,8 +42,6 @@ private:
unsigned int m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
/** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
Matrix6X m_G_centr;
/** Inequality matrix and vector defining the gravito-inertial wrench cone H w <= h */
MatrixXX m_H;
......@@ -222,6 +222,30 @@ public:
*/
LP_status checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max=0.0);
/**
* @brief Check whether the specified com position is in robust equilibrium.
* This amounts to solving the following feasibility LP:
* find b
* minimize 1
* subject to G b = D c + d
* b >= b0
* where:
* b are the coefficient of the contact force generators (f = G b)
* b0 is a parameter proportional to the specified robustness measure
* c is the specified CoM position
* G is the 6xm matrix whose columns are the gravito-inertial wrench generators
* D is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
* d is the 6d vector containing the gravity part of the gravito-inertial wrench
* @param com The 3d center of mass position to test.
* @param acc The 3d acceleration of the CoM.
* @param equilibrium True if com is in robust equilibrium, false otherwise.
* @param e_max Desired robustness level.
* @return The status of the LP solver.
*/
LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max=0.0);
/**
* @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
* This amounts to solving the following LP:
......
......@@ -7,6 +7,8 @@
#include <iostream>
#include <fstream>
#include <cmath>
#include <cassert>
#include <Eigen/Dense>
#include <Eigen/src/Core/util/Macros.h>
......@@ -136,6 +138,61 @@ namespace centroidal_dynamics
std::string getDateAndTimeAsString();
/**
* Computes a binomal coefficient
* @return n!/((n–k)! k!).
*/
value_type nchoosek(const int n, const int k);
template < typename DerivedV, typename DerivedU>
void doCombs(Eigen::Matrix<typename DerivedU::Scalar,1,Eigen::Dynamic>& running,
int& running_i, int& running_j, Eigen::PlainObjectBase<DerivedU> & U, const Eigen::MatrixBase<DerivedV> & V,
int offset, int k)
{
int N = (int)(V.size());
if(k==0)
{
U.row(running_i) = running;
running_i++;
return;
}
for (int i = offset; i <= N - k; ++i)
{
running(running_j) = V(i);
running_j++;
doCombs(running, running_i, running_j, U, V, i+1,k-1);
running_j--;
}
}
/**
* Computes a matrix C containing all possible combinations of the elements of vector v taken k at a time.
* Matrix C has k columns and n!/((n–k)! k!) rows, where n is length(v).
* @param V n-long vector of elements
* @param k size of sub-set to consider
* @param U result matrix
* @return nchoosek by k long matrix where each row is a unique k-size
* the first one, with all zeros.
*/
template < typename DerivedV, typename DerivedU>
void nchoosek(
const Eigen::MatrixBase<DerivedV> & V,
const int k,
Eigen::PlainObjectBase<DerivedU> & U)
{
using namespace Eigen;
if(V.size() == 0)
{
U.resize(0,k);
return;
}
assert((V.cols() == 1 || V.rows() == 1) && "V must be a vector");
U.resize(nchoosek((int)(V.size()),k),k);
int running_i = 0;
int running_j = 0;
Matrix<typename DerivedU::Scalar,1,Dynamic> running(1,k);
doCombs(running, running_i, running_j, U, V,0,k);
}
} //namespace centroidal_dynamics
#endif //_CENTROIDAL_DYNAMICS_LIB_UTIL_HH
......@@ -26,6 +26,25 @@ boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const V
return boost::python::make_tuple(status, robustness);
}
boost::python::tuple wrapCheckRobustEquilibrium(Equilibrium& self, const Vector3& com)
{
bool robustness;
LP_status status = self.checkRobustEquilibrium(com, robustness);
return boost::python::make_tuple(status, robustness);
}
bool wrapSetNewContacts(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
}
bool wrapSetNewContactsFull(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
}
boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self)
{
MatrixXX H;
......@@ -86,17 +105,19 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
/** END enum types **/
bool (Equilibrium::*setNewContacts)
(const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm) = &Equilibrium::setNewContacts;
//bool (Equilibrium::*setNewContacts)
// (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm, const int graspIndex, const double maxGraspForce) = &Equilibrium::setNewContacts;
class_<Equilibrium>("Equilibrium", init<std::string, double, unsigned int, optional <SolverLP, bool, const unsigned int, const bool> >())
.def("useWarmStart", &Equilibrium::useWarmStart)
.def("setUseWarmStart", &Equilibrium::setUseWarmStart)
.def("getName", &Equilibrium::getName)
.def("getAlgorithm", &Equilibrium::getAlgorithm)
.def("setNewContacts", setNewContacts)
.def("setNewContacts", wrapSetNewContacts)
.def("setNewContacts", wrapSetNewContactsFull)
.def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
.def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
.def("checkRobustEquilibrium", wrapCheckRobustEquilibrium)
.def("getPolytopeInequalities", wrapGetPolytopeInequalities)
;
}
......
......@@ -45,3 +45,23 @@ assert (robustness < 0), "first test should NOT be in equilibrirum"
eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
H,h = eq.getPolytopeInequalities()
#~ c= asmatrix(array([0.,0.,1.])).T
status, stable = eq.checkRobustEquilibrium(c)
assert (status == LP_STATUS_OPTIMAL), "checkRobustEquilibrium should not fail"
assert (stable), "lat test should be in equilibrirum"
from numpy import array, vstack, zeros, sqrt, cross, matrix, asmatrix
from numpy.linalg import norm
import numpy as np
def compute_w(c, ddc, dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81])):
w1 = m * (ddc - g_vec)
return array(w1.tolist() + (cross(c, w1) + dL).tolist())
def is_stable(H,c=array([0.,0.,1.]), ddc=array([0.,0.,0.]), dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81]), robustness = 0.):
w = compute_w(c, ddc, dL, m, g_vec)
return (H.dot(-w)<=-robustness).all()
assert(is_stable(H))
......@@ -142,6 +142,7 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
// project generators in 6d centroidal space
m_G_centr.block(0,cg*i,6,cg) = A * G;
}
// Compute the coefficient to convert b0 to e_max
Vector3 f0 = Vector3::Zero();
for(int j=0; j<cg; j++)
......@@ -173,7 +174,6 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
assert(contactPoints.rows()==contactNormals.rows());
if(alg==EQUILIBRIUM_ALGORITHM_IP)
{
SEND_ERROR_MSG("Algorithm IP not implemented yet");
......@@ -190,7 +190,7 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
// Lists of contact generators (3 X generatorsPerContact)
m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient,false))
if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient, false))
{
return false;
}
......@@ -351,13 +351,22 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
return lpStatus_dual;
}
SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
SEND_ERROR_MSG("computeEquilibriumRobustness is not implemented for the specified algorithm");
return LP_STATUS_ERROR;
}
LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max){
checkRobustEquilibrium(com,zero_acc,equilibrium,e_max);
}
LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max)
{
// Take the acceleration in account in D and d :
m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
m_d.head<3>()= m_mass * (m_gravity - acc);
m_HD = m_H * m_D;
m_Hd = m_H * m_d;
if(m_G_centr.cols()==0)
{
equilibrium=false;
......@@ -545,14 +554,19 @@ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vecto
SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
return LP_STATUS_ERROR;
}
bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
{
int n = (int)(v.rows());
int m = (int)(v.cols());
// todo: for the moment using ad hoc upper bound = 500 N
int n = (int)v.rows();
int m = (int)v.cols();
if (n>m)
{
SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
return false;
}
// getProfiler().start("eigen_to_cdd");
dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
// getProfiler().stop("eigen_to_cdd");
dd_ErrorType error = dd_NoError;
......@@ -611,7 +625,6 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
return false;
}
return true;
}
......
......@@ -24,11 +24,9 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
mytype value;
dd_NumberType NT = dd_Real;
dd_init(value);
M=dd_CreateMatrix(m_input, d_input);
M->representation=rep;
M->numbtype=NT;
for (i = 0; i < input.rows(); i++)
{
dd_set_d(value, 0);
......@@ -50,7 +48,6 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
set_free(impl_linset);
free(newpos);
}
return M;
}
......@@ -165,6 +162,51 @@ std::string getDateAndTimeAsString()
strftime(buffer,80,"%Y%m%d_%I%M%S",timeinfo);
return std::string(buffer);
}
/*
int fact(const int n)
{
assert(n>=0);
int res = 1;
for (int i=2 ; i <= n ; ++i)
res *= i;
return res;
}
int choosenk(const int n, const int k)
{
return fact(n) / (fact(k) * fact(n - k));
}*/
/* is this faster ?
value_type choosenk(const int n, const int k)
{
if(k>n/2)
return nchoosek(n,n-k);
else if(k==1)
return n;
else
{
double c = 1;
for(int i = 1;i<=k;i++)
c *= (((double)n-k+i)/((double)i));
return std::round(c);
}
}*/
value_type nchoosek(const int n, const int k)
{
if(k>n/2)
return nchoosek(n,n-k);
else if(k==1)
return n;
else
{
value_type c = 1;
for(int i = 1;i<=k;i++)
c *= (((value_type)n-k+i)/((value_type)i));
return round(c);
}
}
} //namespace centroidal_dynamics
......
......@@ -42,4 +42,5 @@ endif ( MSVC )
ADD_TESTCASE(test_static_equilibrium FALSE)
ADD_TESTCASE(test_LP_solvers FALSE)
PKG_CONFIG_USE_DEPENDENCY(test_LP_solvers qpOASES)
......@@ -343,7 +343,7 @@ void test_small_LP()
model.addRow(3, row2Index, row2Value,
1.0, 1.0);
int n = model.getNumCols();
int n = model.getdimVarXs();
int m = model.getNumRows();
cout<<"Problem has "<<n<<" variables and "<<m<<" constraints.\n";
......
Markdown is supported
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