From 0830b58a37d13943c6999da2077d8f68070954a7 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Thu, 9 Mar 2017 15:47:00 +0100
Subject: [PATCH] added equilibrium method
---
python/centroidal_dynamics_python.cpp | 21 ++++++++++++++-
python/test/binding_tests.py | 39 +++++++++++++++++++++++++++
2 files changed, 59 insertions(+), 1 deletion(-)
create mode 100644 python/test/binding_tests.py
diff --git a/python/centroidal_dynamics_python.cpp b/python/centroidal_dynamics_python.cpp
index 180b225..bbc06d0 100644
--- a/python/centroidal_dynamics_python.cpp
+++ b/python/centroidal_dynamics_python.cpp
@@ -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)
;
}
diff --git a/python/test/binding_tests.py b/python/test/binding_tests.py
new file mode 100644
index 0000000..787ec9e
--- /dev/null
+++ b/python/test/binding_tests.py
@@ -0,0 +1,39 @@
+#~ 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"
--
GitLab