Commit 94614438 authored by stevet's avatar stevet
Browse files

[Cleaning] Remove any reference to grasping, which does not work

parent 7e2d1a52
......@@ -66,9 +66,9 @@ private:
/** Coefficient used for converting the robustness measure in Newtons */
double m_b0_to_emax_coefficient;
bool computePolytopeProjection(Cref_matrix6X v, const int graspIndex = -1);
bool computePolytopeProjection(Cref_matrix6X v);
bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
double frictionCoefficient, const int graspIndex, const double maxGraspForce, const bool perturbate = false);
double frictionCoefficient, const bool perturbate = false);
/**
* @brief Given the smallest coefficient of the contact force generators it computes
......@@ -133,13 +133,10 @@ public:
* @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
* @param frictionCoefficient The contact friction coefficient.
* @param alg Algorithm to use for testing equilibrium.
* @param graspIndex if different than -1, specify starting where the contact points are grasping points
* WARNING graspIndexOnly works for PP algorithm
* @param maxGraspForce maximum grasping force applicable at each contact point
* @return True if the operation succeeded, false otherwise.
*/
bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex = -1, const double maxGraspForce = 50.);
const double frictionCoefficient, const EquilibriumAlgorithm alg);
/**
* @brief Specify a new set of contacts.
......@@ -150,13 +147,10 @@ public:
* @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
* @param frictionCoefficient The contact friction coefficient.
* @param alg Algorithm to use for testing equilibrium.
* @param graspIndex if different than -1, specify starting where the contact points are grasping points
* WARNING graspIndexOnly works for PP algorithm
* @param maxGraspForce maximum grasping force applicable at each contact point
* @return True if the operation succeeded, false otherwise.
*/
bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex = -1, const double maxGraspForce = 500.);
const double frictionCoefficient, const EquilibriumAlgorithm alg);
void setG(Cref_matrix6X G){m_G_centr = G;}
......
......@@ -115,7 +115,7 @@ namespace centroidal_dynamics
* @return The mX(n+1) output cdd matrix, which contains an additional column,
* the first one, with all zeros.
*/
dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize=false, const int graspIndex = -1);
dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize=false);
/**
* Compute the cross-product skew-symmetric matrix associated to the specified vector.
......
......@@ -40,9 +40,9 @@ bool wrapSetNewContacts(Equilibrium& self, const MatrixX3ColMajor& contactPoints
}
bool wrapSetNewContactsFull(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex, const double maxGraspForce)
const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg, graspIndex, maxGraspForce);
return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
}
boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self)
......
......@@ -45,15 +45,10 @@ 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()
#now, use polytope projection algorithm with grasping
eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP,4,50.)
H,h = eq.getPolytopeInequalities()
print P
c= asmatrix(array([0.,0.,1.]))
#~ 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"
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
......
......@@ -79,13 +79,9 @@ Equilibrium::Equilibrium(const string& name, const double mass, const unsigned i
}
bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
double frictionCoefficient, const int graspIndex, const double maxGraspForce, const bool perturbate)
double frictionCoefficient, const bool perturbate)
{
if(graspIndex != -1)
{
assert(graspIndex < contactPoints.rows());
}
long int c = graspIndex > -1 ? graspIndex : contactPoints.rows();
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);
......@@ -146,29 +142,6 @@ 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;
}
if(graspIndex > -1)
{
Matrix3X cube(3, 8);
cube.col(0)= Vector3(-maxGraspForce,-maxGraspForce,-maxGraspForce);
cube.col(1)= Vector3( maxGraspForce,-maxGraspForce,-maxGraspForce);
cube.col(2)= Vector3( maxGraspForce, maxGraspForce,-maxGraspForce);
cube.col(3)= Vector3(-maxGraspForce, maxGraspForce,-maxGraspForce);
cube.col(4)= Vector3(-maxGraspForce,-maxGraspForce, maxGraspForce);
cube.col(5)= Vector3( maxGraspForce,-maxGraspForce, maxGraspForce);
cube.col(6)= Vector3( maxGraspForce, maxGraspForce, maxGraspForce);
cube.col(7)= Vector3(-maxGraspForce, maxGraspForce, maxGraspForce);
//first compute the cube constraining the grasping force.
for(long int i=graspIndex; i<contactPoints.rows(); i++)
{
// compute tangent directions
//N = contactNormals.row(i);
P = contactPoints.row(i);
// compute matrix mapping contact forces to gravito-inertial wrench
A.bottomRows<3>() = crossMatrix(-1.0*P);
// project generators in 6d centroidal space
m_G_centr.block(0,graspIndex*cg + 8*(i-graspIndex),6,8) = A * cube;
}
}
// Compute the coefficient to convert b0 to e_max
Vector3 f0 = Vector3::Zero();
......@@ -190,22 +163,17 @@ void Equilibrium::setAlgorithm(EquilibriumAlgorithm algorithm){
}
bool Equilibrium::setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex, const double maxGraspForce)
const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
MatrixX3 _contactPoints = contactPoints;
MatrixX3 _contactNormals = contactNormals;
return setNewContacts(_contactPoints,_contactNormals, frictionCoefficient, alg, graspIndex, maxGraspForce);
return setNewContacts(_contactPoints,_contactNormals, frictionCoefficient, alg);
}
bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex, const double maxGraspForce)
const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
assert(contactPoints.rows()==contactNormals.rows());
if(alg!=EQUILIBRIUM_ALGORITHM_PP && graspIndex != -1)
{
SEND_ERROR_MSG("GRASPING not implemented yet expect for EQUILIBRIUM_ALGORITHM_PP");
return false;
}
if(alg==EQUILIBRIUM_ALGORITHM_IP)
{
SEND_ERROR_MSG("Algorithm IP not implemented yet");
......@@ -220,12 +188,9 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
m_algorithm = alg;
// Lists of contact generators (3 X generatorsPerContact)
if(graspIndex == -1)
m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
else
m_G_centr.resize(6,graspIndex*m_generatorsPerContact+(contactPoints.rows()-graspIndex)*8);
m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient, graspIndex, maxGraspForce,false))
if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient, false))
{
return false;
}
......@@ -233,9 +198,9 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
if(m_algorithm==EQUILIBRIUM_ALGORITHM_PP)
{
unsigned int attempts = max_num_cdd_trials;
while(!computePolytopeProjection(m_G_centr, graspIndex) && attempts>0)
while(!computePolytopeProjection(m_G_centr) && attempts>0)
{
computeGenerators(contactPoints,contactNormals,frictionCoefficient, graspIndex, maxGraspForce,true);
computeGenerators(contactPoints,contactNormals,frictionCoefficient,true);
attempts--;
}
// resetting random because obviously libcdd always resets to 0
......@@ -589,7 +554,7 @@ 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, const int graspIndex)
bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
{
// todo: for the moment using ad hoc upper bound = 500 N
......@@ -601,7 +566,7 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v, const int graspInde
return false;
}
// getProfiler().start("eigen_to_cdd");
dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix,graspIndex*6);
dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
// getProfiler().stop("eigen_to_cdd");
dd_ErrorType error = dd_NoError;
......
......@@ -12,7 +12,7 @@
namespace centroidal_dynamics
{
dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize, const int graspIndex)
dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize)
{
dd_debug = false;
dd_MatrixPtr M=NULL;
......@@ -27,41 +27,15 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
M=dd_CreateMatrix(m_input, d_input);
M->representation=rep;
M->numbtype=NT;
if(graspIndex > 0)
for (i = 0; i < input.rows(); i++)
{
for (i = graspIndex; i < input.rows(); i++)
{
dd_set_d(value, 1);
dd_set(M->matrix[i-graspIndex][0],value);
for (j = 1; j < d_input; j++)
{
dd_set_d(value, input(i,j-1));
dd_set(M->matrix[i][j],value);
}
}
for (i = 0; i < graspIndex; i++)
{
dd_set_d(value, 0);
dd_set(M->matrix[i][0],value);
for (j = 1; j < d_input; j++)
{
dd_set_d(value, input(i,j-1));
dd_set(M->matrix[i][j],value);
}
}
}
else
{
for (i = 0; i < input.rows(); i++)
{
dd_set_d(value, 0);
dd_set(M->matrix[i][0],value);
for (j = 1; j < d_input; j++)
{
dd_set_d(value, input(i,j-1));
dd_set(M->matrix[i][j],value);
}
}
dd_set_d(value, 0);
dd_set(M->matrix[i][0],value);
for (j = 1; j < d_input; j++)
{
dd_set_d(value, input(i,j-1));
dd_set(M->matrix[i][j],value);
}
}
dd_clear(value);
if(canonicalize)
......
......@@ -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";
......
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