Commit 4decf74e authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added static equilibrium robustness in python

parent 0830b58a
......@@ -12,6 +12,13 @@ namespace centroidal_dynamics
{
using namespace boost::python;
boost::python::tuple wrapComputeQuasiEquilibriumRobustness(Equilibrium& self, const Vector3& com)
{
double robustness;
LP_status status = self.computeEquilibriumRobustness(com, robustness);
return boost::python::make_tuple(status, robustness);
}
boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const Vector3& com, const Vector3& acc)
{
double robustness;
......@@ -74,6 +81,7 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
.def("getName", &Equilibrium::getName)
.def("getAlgorithm", &Equilibrium::getAlgorithm)
.def("setNewContacts", setNewContacts)
.def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
.def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
;
}
......
#~ from centroidal_dynamics import Equilibrium, SolverLP, EquilibriumAlgorithm
from centroidal_dynamics import *
#testing constructors
......@@ -9,11 +8,13 @@ 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
#whether useWarmStart is enable (True by default)
previous = eq.useWarmStart()
#enable warm start in solver (only for QPOases)
eq.setUseWarmStart(False)
assert(previous != eq.useWarmStart())
#access solver name
assert(eq.getName() == "test")
......@@ -24,15 +25,17 @@ 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)]))
#setting contact positions and normals, as well as friction coefficients
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)
#computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
status, robustness = eq.computeEquilibriumRobustness(c)
assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
assert (robustness > 0), "first test should be in equilibrirum"
#computing robustness of a given configuration with non zero acceleration
ddc= asmatrix(array([1000.,0.,0.]))
status, robustness = eq.computeEquilibriumRobustness(c,ddc)
assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
......
......@@ -182,9 +182,19 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
return true;
}
static const Vector3 zero_acc = Vector3::Zero();
LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &robustness)
{
return computeEquilibriumRobustness(com, zero_acc, robustness);
}
LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness)
{
// 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);
const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
if(m==0)
return LP_STATUS_INFEASIBLE;
......@@ -311,20 +321,6 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &ro
return LP_STATUS_ERROR;
}
LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness){
// Take the acceleration in account in D and d :
Matrix63 old_D = m_D;
Vector6 old_d = m_d;
m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
m_d.head<3>()= m_mass * (m_gravity - acc);
// compute equilibrium robustness with the new D and d
LP_status status = computeEquilibriumRobustness(com,robustness);
// Switch back to the original values of D and d
m_D = old_D;
m_d = old_d;
return status;
}
/**
m_d.setZero();
m_d.head<3>() = m_mass*m_gravity;
......
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