bool BCCoreSiconos::callSolver(MatrixX& Mlcp, VectorX& b, VectorX& solution, VectorX& contactIndexToMu, ofstream& os)
{
#ifdef BUILD_BCPLUGIN_WITH_SICONOS
int NC3 = Mlcp.rows();
if(NC3<=0) return true;
int NC = NC3/3;
int CFS_DEBUG = 0;
int CFS_DEBUG_VERBOSE = 0;
if(CFS_DEBUG)
{
if(NC3%3 != 0 ){ os << " warning-1 " << std::endl;return false;}
if( b.rows()!= NC3){ os << " warning-2 " << std::endl;return false;}
if(solution.rows()!= NC3){ os << " warning-3 " << std::endl;return false;}
}
for(int ia=0;ia<NC;ia++)for(int i=0;i<3;i++)prob->q [3*ia+i]= b(((i==0)?(ia):(2*ia+i+NC-1)));
for(int ia=0;ia<NC;ia++) prob->mu[ ia ]= contactIndexToMu[ia];
prob->numberOfContacts = NC;
if( USE_FULL_MATRIX )
{
prob->M->storageType = 0;
prob->M->size0 = NC3;
prob->M->size1 = NC3;
double* ptmp = prob->M->matrix0 ;
for(int ia=0;ia<NC;ia++)for(int i =0;i <3 ;i ++)
{
for(int ja=0;ja<NC;ja++)for(int j =0;j <3;j ++)
{
ptmp[NC3*(3*ia+i)+(3*ja+j)]=Mlcp(((i==0)?(ia):(2*ia+i+NC-1)),((j==0)?(ja):(2*ja+j+NC-1)));
}
}
}
else
{
prob->M->storageType = 1;
prob->M->size0 = NC3;
prob->M->size1 = NC3;
sparsify_A( prob->M->matrix1 , Mlcp , NC , &os);
}
fc3d_driver(prob,reaction,velocity,solops, numops);
double* prea = reaction ;
for(int ia=0;ia<NC;ia++)for(int i=0;i<3;i++) solution(((i==0)?(ia):(2*ia+i+NC-1))) = prea[3*ia+i] ;
if(CFS_DEBUG_VERBOSE)
{
os << "=---------------------------------="<< std::endl;
os << "| res_error =" << solops->dparam[1] << std::endl;
os << "=---------------------------------="<< std::endl;
}
#endif
return true;
}
void HumanoidFootConstraint<Scalar>::computeBoundsVectors(const VectorX& x0)
{
int M = feetSupervisor_.getNbPreviewedSteps();
assert(x0.rows()==2*M);
supBound_.setConstant(2*M, Constant<Scalar>::MAXIMUM_BOUND_VALUE);
infBound_.setConstant(2*M, -Constant<Scalar>::MAXIMUM_BOUND_VALUE);
for(int i = 0; i<M; ++i)
{
const ConvexPolygon<Scalar>& cp = feetSupervisor_.getKinematicConvexPolygon(i);
supBound_(i) = cp.getXSupBound();
supBound_(i + M) = cp.getYSupBound();
infBound_(i) = cp.getXInfBound();
infBound_(i + M) = cp.getYInfBound();
// This if-else statement adds the required terms to vectors supBound_ and infBound_
// so that constraints are expressed in world frame
if(i==0)
{
supBound_(i) += feetSupervisor_.getSupportFootStateX()(0);
supBound_(i + M) += feetSupervisor_.getSupportFootStateY()(0);
infBound_(i) += feetSupervisor_.getSupportFootStateX()(0);
infBound_(i + M) += feetSupervisor_.getSupportFootStateY()(0);
}
else
{
supBound_(i) += x0(i - 1);
supBound_(i + M) += x0(M + i - 1);
infBound_(i) += x0(i - 1);
infBound_(i + M) += x0(M + i - 1);
}
}
// As we optimize a variation dX_ of the QP variable X_ (such that X_ = x0 + dX_),
// the bound constraints need to be translated of x0 too.
supBound_ -= x0;
infBound_ -= x0;
}