Commit 583fa92c authored by t steve's avatar t steve Committed by Pierre Fernbach
Browse files

bug fix index

parent a22ddc4e
......@@ -153,7 +153,7 @@ public:
* @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 = 50.);
const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex = -1, const double maxGraspForce = 500.);
void setG(Cref_matrix6X G){m_G_centr = G;}
......
......@@ -362,7 +362,7 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
return lpStatus_dual;
}
SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
SEND_ERROR_MSG("computeEquilibriumRobustness is not implemented for the specified algorithm");
return LP_STATUS_ERROR;
}
......@@ -568,7 +568,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);
dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix,graspIndex*6);
// getProfiler().stop("eigen_to_cdd");
dd_ErrorType error = dd_NoError;
......
......@@ -24,32 +24,31 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
mytype value;
dd_NumberType NT = dd_Real;
dd_init(value);
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][0],value);
for (j = graspIndex+1; j < d_input; j++)
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);
dd_set_d(value, input(i,j-1));
dd_set(M->matrix[i][j],value);
}
}
for (i = 0; i < input.rows(); i++)
{
dd_set_d(value, 0);
dd_set(M->matrix[i][0],value);
for (j = 1; j < graspIndex+1; j++)
}
for (i = 0; i < graspIndex; i++)
{
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);
}
}
}
}
else
{
......
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