From 6c0d872530f5c24c81db5ca459b6a2c1f51ccb80 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Tue, 9 Aug 2016 16:04:07 +0200
Subject: [PATCH] added perturbation method to recompute invalid matrix
---
.../static_equilibrium.hh | 10 +-
src/static_equilibrium.cpp | 146 +++++++++++-------
2 files changed, 101 insertions(+), 55 deletions(-)
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 4ed0471..04c7629 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -46,6 +46,10 @@ private:
VectorX m_h;
/** False if a numerical instability appeared in the computation H and h*/
bool m_is_cdd_stable;
+ /** STATIC_EQUILIBRIUM_ALGORITHM_PP: If double description fails,
+ * indicate the max number of attempts to compute the cone by introducing
+ * a small pertubation of the system */
+ const unsigned max_num_cdd_trials;
/** Inequality matrix and vector defining the CoM support polygon HD com + Hd <= h */
MatrixX3 m_HD;
@@ -59,6 +63,8 @@ private:
double m_b0_to_emax_coefficient;
bool computePolytopeProjection(Cref_matrix6X v);
+ bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
+ double frictionCoefficient, const bool perturbate = false);
/**
* @brief Given the smallest coefficient of the contact force generators it computes
@@ -81,9 +87,11 @@ public:
* @param generatorsPerContact Number of generators used to approximate the friction cone per contact point.
* @param solver_type Type of LP solver to use.
* @param useWarmStart Whether the LP solver can warm start the resolution.
+ * @param max_num_cdd_trials indicate the max number of attempts to compute the cone by introducing
+ * a small pertubation of the system
*/
StaticEquilibrium(std::string name, double mass, unsigned int generatorsPerContact,
- SolverLP solver_type, bool useWarmStart=true);
+ SolverLP solver_type, bool useWarmStart=true, const unsigned int max_num_cdd_trials=0);
/**
* @brief Returns the useWarmStart flag.
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 35d6b40..635593b 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -18,13 +18,16 @@ namespace robust_equilibrium
bool StaticEquilibrium::m_is_cdd_initialized = false;
StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int generatorsPerContact,
- SolverLP solver_type, bool useWarmStart)
+ SolverLP solver_type, bool useWarmStart,
+ const unsigned int max_num_cdd_trials)
: m_is_cdd_stable(true)
+ , max_num_cdd_trials(max_num_cdd_trials)
{
if(!m_is_cdd_initialized)
{
init_cdd_library();
m_is_cdd_initialized = true;
+ srand ( (unsigned int) (time(NULL)) );
}
if(generatorsPerContact<3)
@@ -49,6 +52,82 @@ StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int gene
m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
}
+bool StaticEquilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
+ double frictionCoefficient, const bool perturbate)
+{
+ long int c = contactPoints.rows();
+ unsigned int &cg = m_generatorsPerContact;
+ double theta, delta_theta=2*M_PI/cg;
+ const value_type pi_4 = value_type(M_PI_4);
+ // perturbation for libcdd
+ const value_type epsilon = 2*1e-3;
+ if(perturbate)
+ frictionCoefficient = frictionCoefficient +(rand()/ value_type(RAND_MAX))*epsilon;
+ // Tangent directions
+ Vector3 T1, T2, N;
+ // contact point
+ Vector3 P;
+ // Matrix mapping a 3d contact force to gravito-inertial wrench (6 X 3)
+ Matrix63 A;
+ A.topRows<3>() = -Matrix3::Identity();
+ Matrix3X G(3, cg);
+ for(long int i=0; i<c; i++)
+ {
+ // check that contact normals have norm 1
+ if(fabs(contactNormals.row(i).norm()-1.0)>1e-6)
+ {
+ SEND_ERROR_MSG("Contact normals should have norm 1, this has norm %f"+toString(contactNormals.row(i).norm()));
+ return false;
+ }
+ // compute tangent directions
+ N = contactNormals.row(i);
+ P = contactPoints.row(i);
+ if(perturbate)
+ {
+ for(int k =0; k<3; ++k)
+ {
+ N(k) +=(rand()/ value_type(RAND_MAX))*epsilon;
+ P(k) +=(rand()/ value_type(RAND_MAX))*epsilon;
+ }
+ N.normalize();
+ }
+ T1 = N.cross(Vector3::UnitY());
+ if(T1.norm()<1e-5)
+ T1 = N.cross(Vector3::UnitX());
+ T2 = N.transpose().cross(T1);
+ T1.normalize();
+ T2.normalize();
+
+ // compute matrix mapping contact forces to gravito-inertial wrench
+ A.bottomRows<3>() = crossMatrix(-1.0*P);
+
+ // compute generators
+ theta = pi_4;
+ for(int j=0; j<cg; j++)
+ {
+ G.col(j) = frictionCoefficient*sin(theta)*T1
+ + frictionCoefficient*cos(theta)*T2
+ + contactNormals.row(i).transpose();
+ G.col(j).normalize();
+ // SEND_DEBUG_MSG("Contact "+toString(i)+" generator "+toString(j)+" = "+toString(G.col(j).transpose()));
+ theta += delta_theta;
+ }
+
+ // 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++)
+ f0 += G.col(j); // sum of the contact generators
+ // Compute the distance between the friction cone boundaries and
+ // the sum of the contact generators, which is e_max when b0=1.
+ // When b0!=1 we just multiply b0 times this value.
+ // This value depends only on the number of generators and the friction coefficient
+ m_b0_to_emax_coefficient = (f0.cross(G.col(0))).norm();
+ return true;
+}
+
bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
double frictionCoefficient, StaticEquilibriumAlgorithm alg)
{
@@ -67,67 +146,26 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
m_algorithm = alg;
- long int c = contactPoints.rows();
- unsigned int &cg = m_generatorsPerContact;
- double theta, delta_theta=2*M_PI/cg;
- // Tangent directions
- Vector3 T1, T2;
- // Matrix mapping a 3d contact force to gravito-inertial wrench (6 X 3)
- Matrix63 A;
- A.topRows<3>() = -Matrix3::Identity();
// Lists of contact generators (3 X generatorsPerContact)
- Matrix3X G(3, cg);
- m_G_centr.resize(6,c*cg);
+ m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
- for(long int i=0; i<c; i++)
+ if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient,false))
{
- // check that contact normals have norm 1
- if(fabs(contactNormals.row(i).norm()-1.0)>1e-6)
- {
- SEND_ERROR_MSG("Contact normals should have norm 1, this has norm %f"+toString(contactNormals.row(i).norm()));
- return false;
- }
- // compute tangent directions
- T1 = contactNormals.row(i).cross(Vector3::UnitY());
- if(T1.norm()<1e-5)
- T1 = contactNormals.row(i).cross(Vector3::UnitX());
- T2 = contactNormals.row(i).transpose().cross(T1);
- T1.normalize();
- T2.normalize();
-
- // compute matrix mapping contact forces to gravito-inertial wrench
- A.bottomRows<3>() = crossMatrix(-1.0*contactPoints.row(i).transpose());
-
- // compute generators
- theta = acos(1/sqrt(2));
- for(int j=0; j<cg; j++)
- {
- G.col(j) = frictionCoefficient*sin(theta)*T1
- + frictionCoefficient*cos(theta)*T2
- + contactNormals.row(i).transpose();
- G.col(j).normalize();
-// SEND_DEBUG_MSG("Contact "+toString(i)+" generator "+toString(j)+" = "+toString(G.col(j).transpose()));
- theta += delta_theta;
- }
-
- // project generators in 6d centroidal space
- m_G_centr.block(0,cg*i,6,cg) = A * G;
+ return false;
}
- // Compute the coefficient to convert b0 to e_max
- Vector3 f0 = Vector3::Zero();
- for(int j=0; j<cg; j++)
- f0 += G.col(j); // sum of the contact generators
- // Compute the distance between the friction cone boundaries and
- // the sum of the contact generators, which is e_max when b0=1.
- // When b0!=1 we just multiply b0 times this value.
- // This value depends only on the number of generators and the friction coefficient
- m_b0_to_emax_coefficient = (f0.cross(G.col(0))).norm();
-
if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_PP)
{
- if(!computePolytopeProjection(m_G_centr))
+ unsigned int attempts = max_num_cdd_trials;
+ while(!computePolytopeProjection(m_G_centr) && attempts>0)
+ {
+ computeGenerators(contactPoints,contactNormals,frictionCoefficient,true);
+ attempts--;
+ }
+ if(!m_is_cdd_stable)
+ {
return false;
+ }
m_HD = m_H * m_D;
m_Hd = m_H * m_d;
}
--
GitLab