Commit 0830b58a authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added equilibrium method

parent 38c09109
......@@ -12,6 +12,13 @@ namespace centroidal_dynamics
{
using namespace boost::python;
boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const Vector3& com, const Vector3& acc)
{
double robustness;
LP_status status = self.computeEquilibriumRobustness(com, acc, robustness);
return boost::python::make_tuple(status, robustness);
}
BOOST_PYTHON_MODULE(centroidal_dynamics)
{
......@@ -19,6 +26,7 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
eigenpy::enableEigenPy();
eigenpy::enableEigenPySpecific<MatrixX3ColMajor,MatrixX3ColMajor>();
eigenpy::enableEigenPySpecific<Vector3,Vector3>();
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
......@@ -46,16 +54,27 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
.value("EQUILIBRIUM_ALGORITHM_DIP", EQUILIBRIUM_ALGORITHM_DIP)
.export_values();
enum_<LP_status>("LP_status")
.value("LP_STATUS_UNKNOWN", LP_STATUS_UNKNOWN)
.value("LP_STATUS_OPTIMAL", LP_STATUS_OPTIMAL)
.value("LP_STATUS_INFEASIBLE", LP_STATUS_INFEASIBLE)
.value("LP_STATUS_UNBOUNDED", LP_STATUS_UNBOUNDED)
.value("LP_STATUS_MAX_ITER_REACHED", LP_STATUS_MAX_ITER_REACHED)
.value("LP_STATUS_ERROR", LP_STATUS_ERROR)
.export_values();
/** END enum types **/
bool (Equilibrium::*setNewContacts)
(const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm) = &Equilibrium::setNewContacts;
/** END enum types **/
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("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
;
}
......
#~ from centroidal_dynamics import Equilibrium, SolverLP, EquilibriumAlgorithm
from centroidal_dynamics import *
#testing constructors
eq = Equilibrium("test", 54., 4)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES )
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES )
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True )
#testing all methods
previous = eq.useWarmStart()
eq.setUseWarmStart(False)
assert(previous != eq.useWarmStart())
assert(eq.getName() == "test")
# creating contact points
from numpy import array, asmatrix, matrix
z = array([0.,0.,1.])
P = asmatrix(array([array([x,y,0]) for x in [-0.05,0.05] for y in [-0.1,0.1]]))
N = asmatrix(array([z for _ in range(4)]))
eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
c= asmatrix(array([0.,0.,1.]))
ddc= asmatrix(array([0.,0.,0.]))
status, robustness = eq.computeEquilibriumRobustness(c,ddc)
assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
assert (robustness > 0), "first test should be in equilibrirum"
ddc= asmatrix(array([1000.,0.,0.]))
status, robustness = eq.computeEquilibriumRobustness(c,ddc)
assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
assert (robustness < 0), "first test should NOT be in equilibrirum"
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