int main(){
bool result = true;
vector<Vector4f> foot_print;
vector<float> cog_list_t, cog_list_x, cog_list_y;
Matrix<float,3,2> xk(Matrix<float,3,2>::Zero());
ZMPPC pc;
foot_print.push_back(Vector4f(0.0, 0.0, 0.0, 0.0));
foot_print.push_back(Vector4f(0.3, 0.0, 0.05, 0.0));
foot_print.push_back(Vector4f(0.6, 0.01, -0.05, 0.0));
foot_print.push_back(Vector4f(0.9, 0.02, 0.05, 0.0));
foot_print.push_back(Vector4f(1.2, 0.02, 0.0, 0.0));
foot_print.push_back(Vector4f(3.2, 0.02, 0.0, 0.0));
pc.target_zmp_interpolation(foot_print);
while(result){
result = pc.calc_xk(xk);
cog_list_x.push_back(xk(0));
cog_list_y.push_back(xk(3));
cog_list_t.push_back(xk(pc.loop_step * dt));
}
plt::plot( pc.refzmp_x, pc.refzmp_y, "-");
plt::plot( cog_list_x, cog_list_y, "-");
plt::legend();
plt::xlabel("y");
plt::xlim( -0.02, 0.04);
plt::ylim( -0.1, 0.1);
plt::show();
}
double
IntervalGramian<WBASIS>::norm_Ainv() const
{
if (normAinv == 0.0) {
typedef typename WaveletBasis::Index Index;
std::set<Index> Lambda;
const int j0 = basis().j0();
const int jmax = 8;
for (Index lambda = first_generator(&basis(), j0);; ++lambda) {
Lambda.insert(lambda);
if (lambda == last_wavelet(&basis(), jmax)) break;
}
SparseMatrix<double> A_Lambda;
setup_stiffness_matrix(*this, Lambda, A_Lambda);
#if 1
double help;
unsigned int iterations;
LanczosIteration(A_Lambda, 1e-6, help, normA, 200, iterations);
normAinv = 1./help;
#else
Vector<double> xk(Lambda.size(), false);
xk = 1;
unsigned int iterations;
normAinv = InversePowerIteration(A_Lambda, xk, 1e-6, 200, iterations);
#endif
}
return normAinv;
}
int main(int argc, char *argv[]) {
clock_t fclock, nclock;
int i;
double *data = (double*) fftw_malloc(sizeof(double) * (2*LITN+1));
double *naiveResult = (double*) fftw_malloc(sizeof(double) * BIGN);
double *fancyResult = (double*) fftw_malloc(sizeof(double) * BIGN);
// Load input data
oneDFileReader("data/input.txt", (2*LITN+1), data);
for(i=0; i<=2*LITN; ++i) {
data[i] *= exp(0 - pow(xk(i), 2) / 2);
}
// Perform transformations
initFastFouriers(BIGN);
initRns(BIGN);
nclock = clock();
naiveTransform(data, naiveResult);
nclock -= clock();
fclock = clock();
oneDTransform(data, fancyResult);
fclock -= clock();
destroyFastFouriers(BIGN);
// Save output data
FILE *resultsn;
FILE *resultsf;
resultsn = fopen("data/result_n.txt", "wt+");
resultsf = fopen("data/result_f.txt", "wt+");
for(i=0; i<BIGN; ++i){
fancyResult[i] *= (2*BIGC) / LITN;
naiveResult[i] *= (2*BIGC) / LITN;
fprintf(resultsn, "%.16lf\n", naiveResult[i]);
fprintf(resultsf, "%.16lf\n", fancyResult[i]);
}
fclose(resultsn);
fclose(resultsf);
fprintf(stdout, "Naive transform took %li\n", -nclock);
fprintf(stdout, "Fancy transform took %li\n", -fclock);
fprintf(stdout, "Clocks per second: %li\n", CLOCKS_PER_SEC);
return 0;
}
double
SturmEquation<WBASIS>::norm_A() const
{
if (normA == 0.0) {
typedef typename WaveletBasis::Index Index;
std::set<Index> Lambda;
const int j0 = basis().j0();
const int jmax = j0+3;
#ifdef FRAME
const int pmax = std::min(basis().get_pmax_(),2);
//const int pmax = 0;
int p = 0;
for (Index lambda = basis().first_generator(j0,0);;) {
Lambda.insert(lambda);
if (lambda == basis().last_wavelet(jmax,pmax)) break;
//if (i==7) break;
if (lambda == basis().last_wavelet(jmax,p)) {
++p;
lambda = basis().first_generator(j0,p);
}
else
++lambda;
}
#else
for (Index lambda = first_generator(&basis(), j0);; ++lambda) {
Lambda.insert(lambda);
if (lambda == last_wavelet(&basis(), jmax)) break;
}
#endif
SparseMatrix<double> A_Lambda;
setup_stiffness_matrix(*this, Lambda, A_Lambda);
#if 1
double help;
unsigned int iterations;
LanczosIteration(A_Lambda, 1e-6, help, normA, 200, iterations);
normAinv = 1./help;
#else
Vector<double> xk(Lambda.size(), false);
xk = 1;
unsigned int iterations;
normA = PowerIteration(A_Lambda, xk, 1e-6, 100, iterations);
#endif
}
return normA;
}
void TPZHelmholtzComplex1D::Contribute(TPZMaterialData &data, REAL weight, TPZFMatrix<STATE> &ek, TPZFMatrix<STATE> &ef) {
TPZManVector<STATE,2> alphaval(1), betaval(1), phiaval(1);
fAlpha->Execute(data.x, alphaval);
fBeta->Execute(data.x, betaval);
fPhi->Execute(data.x, phiaval);
#ifdef LOG4CXX
{
std::stringstream sout;
sout << "Coordenate x: " << data.x << " alpha = " << alphaval << " beta = " << betaval << " phi = " << phiaval;
LOGPZ_DEBUG(logger, sout.str());
}
#endif
TPZFNMatrix<4, STATE> xk(1, 1), xb(1, 1), xc(1, 1, 0.), xf(1, 1);
xk(0,0) = alphaval[0];
xb(0,0) = betaval[0];
xf(0,0) = -phiaval[0];
SetMaterial(xk, xc, xb, xf);
TPZMat1dLin::Contribute(data, weight, ek, ef);
}
void
LEPlic :: doCellDLS(FloatArray &fvgrad, int ie, bool coord_upd, bool vof_temp_flag)
{
int i, ineighbr, nneighbr;
double fvi, fvk, wk, dx, dy;
bool isBoundaryCell = false;
LEPlicElementInterface *interface, *ineghbrInterface;
FloatMatrix lhs(2, 2);
FloatArray rhs(2), xi(2), xk(2);
IntArray currCell(1), neighborList;
ConnectivityTable *contable = domain->giveConnectivityTable();
if ( ( interface = ( LEPlicElementInterface * ) ( domain->giveElement(ie)->giveInterface(LEPlicElementInterfaceType) ) ) ) {
if ( vof_temp_flag ) {
fvi = interface->giveTempVolumeFraction();
} else {
fvi = interface->giveVolumeFraction();
}
if ( ( fvi > 0. ) && ( fvi <= 1.0 ) ) {
// potentially boundary cell
if ( ( fvi > 0. ) && ( fvi < 1.0 ) ) {
isBoundaryCell = true;
}
/* DLS (Differential least square reconstruction)
*
* In the DLS method, volume fraction Taylor series expansion of vf (volume fraction)
* is formed from each reference cell volume fraction vf at element center x(i) to each
* cell neighbor at point x(k). The sum (vf(i)-vf(k))^2 over all immediate neighbors
* is then minimized inthe least square sense.
*/
// get list of neighbours to current cell including current cell
currCell.at(1) = ie;
contable->giveElementNeighbourList(neighborList, currCell);
// loop over neighbors to assemble normal equations
nneighbr = neighborList.giveSize();
interface->giveElementCenter(this, xi, coord_upd);
lhs.zero();
rhs.zero();
for ( i = 1; i <= nneighbr; i++ ) {
ineighbr = neighborList.at(i);
if ( ineighbr == ie ) {
continue; // skip itself
}
if ( ( ineghbrInterface =
( LEPlicElementInterface * ) ( domain->giveElement(ineighbr)->giveInterface(LEPlicElementInterfaceType) ) ) ) {
if ( vof_temp_flag ) {
fvk = ineghbrInterface->giveTempVolumeFraction();
} else {
fvk = ineghbrInterface->giveVolumeFraction();
}
if ( fvk < 1.0 ) {
isBoundaryCell = true;
}
ineghbrInterface->giveElementCenter(this, xk, coord_upd);
wk = xk.distance(xi);
dx = ( xk.at(1) - xi.at(1) ) / wk;
dy = ( xk.at(2) - xi.at(2) ) / wk;
lhs.at(1, 1) += dx * dx;
lhs.at(1, 2) += dx * dy;
lhs.at(2, 2) += dy * dy;
rhs.at(1) += ( fvi - fvk ) * dx / wk;
rhs.at(2) += ( fvi - fvk ) * dy / wk;
}
}
if ( isBoundaryCell ) {
// symmetry
lhs.at(2, 1) = lhs.at(1, 2);
// solve normal equation for volume fraction gradient
lhs.solveForRhs(rhs, fvgrad);
// compute unit normal
fvgrad.normalize();
fvgrad.negated();
#ifdef __OOFEG
/*
* EASValsSetLayer(OOFEG_DEBUG_LAYER);
* WCRec p[2];
* double tx = -fvgrad.at(2), ty=fvgrad.at(1);
* p[0].x=xi.at(1)-tx*0.1;
* p[0].y=xi.at(2)-ty*0.1;
* p[1].x=xi.at(1)+tx*0.1;
* p[1].y=xi.at(2)+ty*0.1;
* p[0].z = p[1].z = 0.0;
* GraphicObj *go = CreateLine3D(p);
* EGWithMaskChangeAttributes(LAYER_MASK, go);
* EMAddGraphicsToModel(ESIModel(), go);
* ESIEventLoop (YES, "Cell DLS finished; Press Ctrl-p to continue");
*/
#endif
} else {
fvgrad.zero();
}
}
}
}
IGL_INLINE double igl::polyvector_field_poisson_reconstruction(
const Eigen::PlainObjectBase<DerivedV> &Vcut,
const Eigen::PlainObjectBase<DerivedF> &Fcut,
const Eigen::PlainObjectBase<DerivedS> &sol3D_combed,
Eigen::PlainObjectBase<DerivedSF> &scalars,
Eigen::PlainObjectBase<DerivedS> &sol3D_recon,
Eigen::PlainObjectBase<DerivedE> &max_error )
{
Eigen::SparseMatrix<typename DerivedV::Scalar> gradMatrix;
igl::grad(Vcut, Fcut, gradMatrix);
Eigen::VectorXd FAreas;
igl::doublearea(Vcut, Fcut, FAreas);
FAreas = FAreas.array() * .5;
int nf = FAreas.rows();
Eigen::SparseMatrix<typename DerivedV::Scalar> M,M1;
Eigen::VectorXi II = igl::colon<int>(0, nf-1);
igl::sparse(II, II, FAreas, M1);
igl::repdiag(M1, 3, M) ;
int half_degree = sol3D_combed.cols()/3;
sol3D_recon.setZero(sol3D_combed.rows(),sol3D_combed.cols());
int numF = Fcut.rows();
scalars.setZero(Vcut.rows(),half_degree);
Eigen::SparseMatrix<typename DerivedV::Scalar> Q = gradMatrix.transpose()* M *gradMatrix;
//fix one point at Ik=fix, value at fixed xk=0
int fix = 0;
Eigen::VectorXi Ik(1);Ik<<fix;
Eigen::VectorXd xk(1);xk<<0;
//unknown indices
Eigen::VectorXi Iu(Vcut.rows()-1,1);
Iu<<igl::colon<int>(0, fix-1), igl::colon<int>(fix+1,Vcut.rows()-1);
Eigen::SparseMatrix<typename DerivedV::Scalar> Quu, Quk;
igl::slice(Q, Iu, Iu, Quu);
igl::slice(Q, Iu, Ik, Quk);
Eigen::SimplicialLDLT<Eigen::SparseMatrix<typename DerivedV::Scalar> > solver;
solver.compute(Quu);
Eigen::VectorXd vec; vec.setZero(3*numF,1);
for (int i =0; i<half_degree; ++i)
{
vec<<sol3D_combed.col(i*3+0),sol3D_combed.col(i*3+1),sol3D_combed.col(i*3+2);
Eigen::VectorXd b = gradMatrix.transpose()* M * vec;
Eigen::VectorXd bu = igl::slice(b, Iu);
Eigen::VectorXd rhs = bu-Quk*xk;
Eigen::MatrixXd yu = solver.solve(rhs);
Eigen::VectorXi index = i*Eigen::VectorXi::Ones(Iu.rows(),1);
igl::slice_into(yu, Iu, index, scalars);scalars(Ik[0],i)=xk[0];
}
// evaluate gradient of found scalar function
for (int i =0; i<half_degree; ++i)
{
Eigen::VectorXd vec_poisson = gradMatrix*scalars.col(i);
sol3D_recon.col(i*3+0) = vec_poisson.segment(0*numF, numF);
sol3D_recon.col(i*3+1) = vec_poisson.segment(1*numF, numF);
sol3D_recon.col(i*3+2) = vec_poisson.segment(2*numF, numF);
}
max_error.setZero(numF,1);
for (int i =0; i<half_degree; ++i)
{
Eigen::VectorXd diff = (sol3D_recon.block(0, i*3, numF, 3)-sol3D_combed.block(0, i*3, numF, 3)).rowwise().norm();
diff = diff.array() / sol3D_combed.block(0, i*3, numF, 3).rowwise().norm().array();
max_error = max_error.cwiseMax(diff.cast<typename DerivedE::Scalar>());
}
return max_error.mean();
}
// [ref] undistort_images_using_formula() in ${CPP_RND_HOME}/test/machine_vision/opencv/opencv_image_undistortion.cpp
void KinectSensor::computeHomogeneousImageCoordinates(const cv::Size &imageSize, const cv::Mat &K, const cv::Mat &distCoeffs, cv::Mat &IC_homo, cv::Mat &IC_homo_undist)
{
// [ref] "Joint Depth and Color Camera Calibration with Distortion Correction", D. Herrera C., J. Kannala, & J. Heikkila, TPAMI, 2012
// homogeneous image coordinates: zero-based coordinates
cv::Mat(3, imageSize.height * imageSize.width, CV_64FC1, cv::Scalar::all(1)).copyTo(IC_homo);
//IC_homo = cv::Mat::ones(3, imageSize.height * imageSize.width, CV_64FC1);
{
#if 0
// 0 0 0 ... 0 1 1 1 ... 1 ... 639 639 639 ... 639
// 0 1 2 ... 479 0 1 2 ... 479 ... 0 1 2 ... 479
cv::Mat arr(1, imageSize.height, CV_64FC1);
for (int i = 0; i < imageSize.height; ++i)
arr.at<double>(0, i) = (double)i;
for (int i = 0; i < imageSize.width; ++i)
{
IC_homo(cv::Range(0, 1), cv::Range(i * imageSize.height, (i + 1) * imageSize.height)).setTo(cv::Scalar::all(i));
arr.copyTo(IC_homo(cv::Range(1, 2), cv::Range(i * imageSize.height, (i + 1) * imageSize.height)));
}
#else
// 0 1 2 ... 639 0 1 2 ... 639 ... 0 1 2 ... 639
// 0 0 0 ... 0 1 1 1 ... 1 ... 479 479 479 ... 479
cv::Mat arr(1, imageSize.width, CV_64FC1);
for (int i = 0; i < imageSize.width; ++i)
arr.at<double>(0, i) = (double)i;
for (int i = 0; i < imageSize.height; ++i)
{
arr.copyTo(IC_homo(cv::Range(0, 1), cv::Range(i * imageSize.width, (i + 1) * imageSize.width)));
IC_homo(cv::Range(1, 2), cv::Range(i * imageSize.width, (i + 1) * imageSize.width)).setTo(cv::Scalar::all(i));
}
#endif
}
// homogeneous normalized camera coordinates
const cv::Mat CC_norm(K.inv() * IC_homo);
// apply distortion
{
//const cv::Mat xn(CC_norm(cv::Range(0,1), cv::Range::all()));
const cv::Mat xn(CC_norm(cv::Range(0,1), cv::Range::all()) / CC_norm(cv::Range(2,3), cv::Range::all()));
//const cv::Mat yn(CC_norm(cv::Range(1,2), cv::Range::all()));
const cv::Mat yn(CC_norm(cv::Range(1,2), cv::Range::all()) / CC_norm(cv::Range(2,3), cv::Range::all()));
const cv::Mat xn2(xn.mul(xn));
const cv::Mat yn2(yn.mul(yn));
const cv::Mat xnyn(xn.mul(yn));
const cv::Mat r2(xn2 + yn2);
const cv::Mat r4(r2.mul(r2));
const cv::Mat r6(r4.mul(r2));
const double &k1 = distCoeffs.at<double>(0);
const double &k2 = distCoeffs.at<double>(1);
const double &k3 = distCoeffs.at<double>(2);
const double &k4 = distCoeffs.at<double>(3);
const double &k5 = distCoeffs.at<double>(4);
const cv::Mat xg(2.0 * k3 * xnyn + k4 * (r2 + 2.0 * xn2));
const cv::Mat yg(k3 * (r2 + 2.0 * yn2) + 2.0 * k4 * xnyn);
const cv::Mat coeff(1.0 + k1 * r2 + k2 * r4 + k5 * r6);
cv::Mat xk(3, imageSize.height * imageSize.width, CV_64FC1, cv::Scalar::all(1));
cv::Mat(coeff.mul(xn) + xg).copyTo(xk(cv::Range(0,1), cv::Range::all()));
cv::Mat(coeff.mul(yn) + yg).copyTo(xk(cv::Range(1,2), cv::Range::all()));
IC_homo_undist = K * xk;
}
}
double ChLcpInteriorPoint::Solve(
ChLcpSystemDescriptor& sysd ///< system description with constraints and variables
)
{
std::cout << "-------using interior point solver!!------" << std::endl;
std::vector<ChLcpConstraint*>& mconstraints = sysd.GetConstraintsList();
std::vector<ChLcpVariables*>& mvariables = sysd.GetVariablesList();
ChMatrixDynamic <double> mv0;
ChSparseMatrix mM;
ChSparseMatrix mCq;
ChSparseMatrix mE;
ChMatrixDynamic <double> mf;
ChMatrixDynamic <double> mb;
ChMatrixDynamic <double> mfric;
sysd.ConvertToMatrixForm(&mCq, &mM, &mE, &mf, &mb, &mfric);
sysd.FromVariablesToVector(mv0);
ChStreamOutAsciiFile file_V0( "dump_V_old.dat" ) ;
mv0.StreamOUTdenseMatlabFormat(file_V0) ;
ChStreamOutAsciiFile file_M ( "dump_M.dat" ) ;
mM.StreamOUTsparseMatlabFormat ( file_M ) ;
ChStreamOutAsciiFile file_Cq ( "dump_Cq.dat" ) ;
mCq.StreamOUTsparseMatlabFormat ( file_Cq ) ;
ChStreamOutAsciiFile file_E ( "dump_E.dat" ) ;
mE.StreamOUTsparseMatlabFormat ( file_E ) ;
ChStreamOutAsciiFile file_f ( "dump_f.dat" ) ;
mf.StreamOUTdenseMatlabFormat ( file_f ) ;
ChStreamOutAsciiFile file_b ( "dump_b.dat" ) ;
mb.StreamOUTdenseMatlabFormat ( file_b ) ;
ChStreamOutAsciiFile file_fric ( "dump_fric.dat" ) ;
mfric.StreamOUTdenseMatlabFormat ( file_fric ) ;
printf("Successfully writing chickenbutt files!\n");
/* file_f.GetFstream().close();
file_fric.GetFstream().close();
file_V0.GetFstream().close();
file_M.GetFstream().close();
file_Cq.GetFstream().close();
file_b.GetFstream().close();
*/
int nBodies = mM.GetColumns()/6;
size_t nVariables = mvariables.size();
size_t nConstraints = sysd.CountActiveConstraints();
int numContacts = nConstraints/3;
size_t nOfConstraints = mconstraints.size();
/* ALWYAS DO THIS IN THE LCP SOLVER!!!*/
for (unsigned int ic = 0; ic < nConstraints; ic++)
mconstraints[ic]->Update_auxiliary();
//Get sparse info for contact jacobian and Minv matrix to pass on to Ang's solver
std::vector<int> index_i_Cq;
std::vector<int> index_j_Cq;
std::vector<double> val_Cq;
double val;
// fprintf(stderr, "------------Cq(from C::E)----------\n");
for (int ii = 0; ii < mCq.GetRows(); ii++){
for (int jj = 0; jj < mCq.GetColumns(); jj++){
val = mCq.GetElement(ii,jj);
if (val){
index_i_Cq.push_back(jj);
index_j_Cq.push_back(ii);
val_Cq.push_back(val);
// fprintf(stderr, "%d %d %.20g\n", ii, jj, val);
}
}
}
/* for (int iv = 0; iv < mvariables.size(); iv++)
if (mvariables[iv]->IsActive())
mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb());
*/
// int count = 0;
// for (std::vector<int>::iterator it = index_i_Cq.begin(); it != index_i_Cq.end(); it ++){
// std::cout << "(" << index_i_Cq[count] <<"," << index_j_Cq[count] <<"):" << val_Cq[count] << std::endl;
// count ++;
// }
// Minv matrix
std::vector<int> index_i_Minv;
std::vector<int> index_j_Minv;
std::vector<double> val_Minv;
for (int i = 0; i < nBodies*6; i++){
index_i_Minv.push_back(i);
index_j_Minv.push_back(i);
val_Minv.push_back(1.0/mM.GetElement(i,i));
}
// create reference to pass on to SPIKE
int *Cq_i = &index_i_Cq[0];
int *Cq_j = &index_j_Cq[0];
int Cq_nnz = val_Cq.size();
double *Cq_val = &val_Cq[0];
int *Minv_i = &index_i_Minv[0];
int *Minv_j = &index_j_Minv[0];
double *Minv_val = &val_Minv[0];
// formulate rhs of optimization problem f(x) = 1/2 *x'*N*x + r'*x
ChMatrixDynamic <double> opt_r_tmp(nConstraints,1);
// assemble r vector
/** 1. put [M^-1]*k in q sparse vector of each variable **/
for (unsigned int iv = 0; iv < nVariables; iv ++)
if (mvariables[iv]->IsActive()){
mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb());
ChMatrix<double> k = mvariables[iv]->Get_fb();
ChMatrix<double> Mk = mvariables[iv]->Get_qb();
// fprintf(stderr, "Body %d k: %.12f %.12f %.12f\n", iv, k(0,0), k(1,0), k(2,0));
// fprintf(stderr, "Body %d M^[-1]*k: %.12f %.12f %.12f\n", iv, Mk(0,0), Mk(1,0), Mk(2,0));
}
/** 2. now do rhs = D'*q = D'*(M^-1)*k **/
int s_i = 0;
opt_r.Resize(nConstraints,1);
for (unsigned int ic = 0; ic < nConstraints; ic ++)
if (mconstraints[ic]->IsActive()){
opt_r(s_i,0) = mconstraints[ic]->Compute_Cq_q();
++s_i;
}
// fprintf(stderr, "------D'*M^(-1)*k-------\n");
// for (int i = 0; i < opt_r.GetRows(); i++)
// fprintf(stderr, "%.16f\n", opt_r(i,0));
/** 3. rhs = rhs + c **/
sysd.BuildBiVector(opt_r_tmp);
opt_r.MatrInc(opt_r_tmp);
// fprintf(stderr, "------opt_r-------\n");
// for (int i = 0; i < opt_r.GetRows(); i++)
// fprintf(stderr, "%.12f\n", opt_r(i,0));
///////////////////
//velocity update//
///////////////////
ChMatrixDynamic<> mq;
sysd.FromVariablesToVector(mq, true);
for (int i = 0; i < mq.GetRows(); i++){
// mq.SetElementN(i, mf.GetElementN(i)/mM.GetElement(i,i) + mv0.GetElementN(i));
// mq.SetElementN(i, mf.GetElementN(i)/mM.GetElement(i,i));
// fprintf(stderr, "%d: %g / %g + %g = %g\n",i, mf.GetElementN(i), mM.GetElement(i,i), mv0.GetElementN(i), mq.GetElementN(i));
// fprintf(stderr, "%g\n", mq.GetElementN(i));
}
////////////////////////////
//assign solver parameters//
////////////////////////////
double barrier_t = 1;
double eta_hat;
int numStages = 500;
int mu1 = 10;
double b1 = 0.5;
double a1 = 0.01;
// assign vectors here
ff.Resize(numContacts*2,1);
lambda_k.Resize(numContacts*2,1); /*initialize lambda_k*/
xk.Resize(numContacts*3,1);
r_dual.Resize(numContacts*3,1);
r_cent.Resize(numContacts*2,1);
d_x.Resize(numContacts*3,1);
d_lambda.Resize(numContacts*2,1);
Schur_rhs.Resize(3*numContacts,1);
grad_f.Resize(3*numContacts,1);
if (mconstraints.size() == 0){
sysd.FromVectorToConstraints(xk);
sysd.FromVectorToVariables(mq);
for (size_t ic = 0; ic < mconstraints.size(); ic ++){
if (mconstraints[ic]->IsActive())
mconstraints[ic]->Increment_q(mconstraints[ic]->Get_l_i());
}
return 1e-8;
}
double *BlockDiagonal_val = new double[9*numContacts];
int *BlockDiagonal_i = new int[9*numContacts];
int *BlockDiagonal_j = new int[9*numContacts];
double *spike_rhs = new double[3*numContacts];
int tmp0, tmp1, tmp2;
for (int i = 0; i < numContacts; i ++){
tmp0 = 3*i;
tmp1 = 3*i+1;
tmp2 = 3*i+2;
*(BlockDiagonal_i + 9*i) = tmp0;
*(BlockDiagonal_i + 9*i+1) = tmp0;
*(BlockDiagonal_i + 9*i+2) = tmp0;
*(BlockDiagonal_i + 9*i+3) = tmp1;
*(BlockDiagonal_i + 9*i+4) = tmp1;
*(BlockDiagonal_i + 9*i+5) = tmp1;
*(BlockDiagonal_i + 9*i+6) = tmp2;
*(BlockDiagonal_i + 9*i+7) = tmp2;
*(BlockDiagonal_i + 9*i+8) = tmp2;
*(BlockDiagonal_j + 9*i) = tmp0;
*(BlockDiagonal_j + 9*i+1) = tmp1;
*(BlockDiagonal_j + 9*i+2) = tmp2;
*(BlockDiagonal_j + 9*i+3) = tmp0;
*(BlockDiagonal_j + 9*i+4) = tmp1;
*(BlockDiagonal_j + 9*i+5) = tmp2;
*(BlockDiagonal_j + 9*i+6) = tmp0;
*(BlockDiagonal_j + 9*i+7) = tmp1;
*(BlockDiagonal_j + 9*i+8) = tmp2;
}
// initialize xk
for (int i = 0; i < numContacts; i ++){
xk(3*i, 0) = 1;
xk(3*i+1, 0) = 0;
xk(3*i+2, 0) = 0;
}
evaluateConstraints(mfric.GetAddress(), numContacts, false);
//initialize lambda
for (int i = 0; i < lambda_k.GetRows(); i++)
lambda_k(i,0) = -1/(barrier_t * ff(i,0));
/////////////////////////////
////GO THROUGH EACH STAGE////
/////////////////////////////
for (int stage = 0; stage < numStages; stage++){
eta_hat = - lambda_k.MatrDot(&lambda_k, &ff);
barrier_t = mu1 * (2*numContacts)/eta_hat;
// assemble grad_f = N*x + r
sysd.ShurComplementProduct(grad_f, &xk, 0);
// fprintf(stderr, "----------N*x----------\n");
// for (int i = 0; i < grad_f.GetRows(); i++)
// fprintf(stderr, "%.20f\n", grad_f.GetElementN(i));
grad_f.MatrInc(opt_r);
// fprintf(stderr, "----------grad_f----------\n");
// for (int i = 0; i < grad_f.GetRows(); i++)
// fprintf(stderr, "%.20f\n", grad_f.GetElementN(i));
// compute r_d and r_c for schur implementation
computeSchurRHS(grad_f.GetAddress(), mfric.GetAddress(), numContacts, barrier_t);
// fprintf(stderr, "----------r_dual----------\n");
// for (int i = 0; i < r_dual.GetRows(); i++)
// fprintf(stderr, "%.16f\n", r_dual.GetElementN(i));
// fprintf(stderr, "----------r_cent----------\n");
// for (int i = 0; i < r_cent.GetRows(); i++)
// fprintf(stderr, "%.16f\n", r_cent.GetElementN(i));
// assemble block diagonal matrix
computeBlockDiagonal(BlockDiagonal_val, mfric.GetAddress(), numContacts, barrier_t);
// assemble rhs vector for spike solver
computeSpikeRHS(spike_rhs, mfric.GetAddress(), numContacts, barrier_t);
// fprintf(stderr, "----------spike_rhs----------\n");
// for (int i = 0; i < 3*numContacts; i++){
// fprintf(stderr, "%.16f\n", *(spike_rhs + i));
// }
double *spike_dx = new double [3*numContacts];
//call ang's solver here....
bool solveSuc = solveSPIKE(nBodies, numContacts, Cq_i, Cq_j, Cq_nnz, Cq_val, Minv_i, Minv_j, Minv_val, BlockDiagonal_i, BlockDiagonal_j, BlockDiagonal_val, spike_dx, spike_rhs);
if (solveSuc == false)
std::cerr << "Solve Failed!" << std::endl;
// assume d_x is calculated perfectly!
for (int i = 0; i < numContacts; i++){
d_x(3*i,0) = *(spike_dx + 3*i);
d_x(3*i+1,0) = *(spike_dx + 3*i + 1);
d_x(3*i+2,0) = *(spike_dx + 3*i + 2);
}
/* fprintf(stderr, "-------d_x---------\n");
for (int i = 0; i < d_x.GetRows(); i++){
fprintf(stderr, "%.20f\n", d_x(i,0));
}
*/
// free the heap!
delete [] spike_dx;
// evaluate d_lambda
for (int i = 0; i < numContacts; i++){
d_lambda(i) = lambda_k(i,0)/ff(i,0) * (pow(mfric(3*i,0),2)*xk(3*i,0)*d_x(3*i,0) - xk(3*i+1,0)*d_x(3*i+1,0) -xk(3*i+2,0)*d_x(3*i+2,0) - r_cent(i,0) );
d_lambda(i + numContacts) = lambda_k(i+numContacts,0)/ff(i+numContacts)*(d_x(3*i) - r_cent(i + numContacts));
}
/* fprintf(stderr, "----------d_lambda----------\n");
for (int i = 0; i < 2*numContacts; i++){
fprintf(stderr, "%.16f\n", d_lambda(i,0));
}
*/
///////////////
//LINE SEARCH//
///////////////
double s_max = 1;
double tmp;
for (int i = 0; i < 2*numContacts; i ++){
if (d_lambda(i,0) < 0){
tmp = -lambda_k(i,0)/d_lambda(i,0);
// fprintf(stderr, "i = %d, tmp = %.20f\n", i, tmp);
if (tmp < s_max){
s_max = tmp;
}
}
}
double bla = 0.99;
double ss = bla * s_max;
// fprintf(stderr, "s_max = %.20g\n", s_max);
ff_tmp.Resize(2*numContacts,1);
lambda_k_tmp.Resize(2*numContacts,1);
xk_tmp.Resize(3*numContacts,1);;
r_dual_tmp.Resize(3*numContacts,1);;
r_cent_tmp.Resize(3*numContacts,1);;
bool DO = true;
int count = 0;
// fprintf(stderr, "----line search----\n");
while (DO){
xk_tmp = d_x;
// fprintf(stderr, "-----d_x----\n");
// for (int i = 0; i < 3*numContacts; i ++)
// fprintf(stderr, "%.20g\n", xk_tmp(i,0));
xk_tmp.MatrScale(ss);
// fprintf(stderr, "-----ss*d_x----\n");
// for (int i = 0; i < 3*numContacts; i ++)
// fprintf(stderr, "%.20g\n", xk_tmp(i,0));
xk_tmp.MatrAdd(xk,xk_tmp);
// fprintf(stderr, "-----xk+ss*d_x----\n");
// for (int i = 0; i < 3*numContacts; i ++)
// fprintf(stderr, "%.20g\n", xk_tmp(i,0));
evaluateConstraints(mfric.GetAddress(), numContacts, true);
// fprintf(stderr, "-----tmp_ff----\n");
// for (int i = 0; i < 2*numContacts; i ++)
// fprintf(stderr, "%.20g\n", ff_tmp(i,0));
// fprintf(stderr, "max_ff = %.20g\n", ff_tmp.Max());
if (ff_tmp.Max()<0){
DO = false;
}
else{
count++;
ss = b1 * ss;
// fprintf(stderr,"ss[%d] = %.20g\n", count, ss);
}
}
DO = true;
double norm_r_t = sqrt(pow(r_dual.NormTwo(),2) + pow(r_cent.NormTwo(),2));
double norm_r_t_ss;
count = 0;
while (DO){
xk_tmp = d_x;
xk_tmp.MatrScale(ss);
xk_tmp.MatrAdd(xk,xk_tmp);
lambda_k_tmp = d_lambda;
lambda_k_tmp.MatrScale(ss);
lambda_k_tmp.MatrAdd(lambda_k, lambda_k_tmp);
evaluateConstraints(mfric.GetAddress(),numContacts,true);
sysd.ShurComplementProduct(grad_f, &xk_tmp, 0);
grad_f.MatrInc(opt_r);
computeSchurKKT(grad_f.GetAddress(), mfric.GetAddress(), numContacts, barrier_t, true);
norm_r_t_ss = sqrt(pow(r_dual_tmp.NormTwo(),2) + pow(r_cent_tmp.NormTwo(),2));
if (norm_r_t_ss < (1 - a1*ss)*norm_r_t)
DO = false;
else{
count ++;
ss = b1*ss;
// fprintf(stderr,"ss[%d] = %.20g\n", count, ss);
}
}
// upadate xk and lambda_k
d_x.MatrScale(ss);
// fprintf(stderr, "-------ss*d_x---------\n");
// for (int i = 0; i < d_x.GetRows(); i++)
// fprintf(stderr, "%.20f\n", d_x(i,0));
// fprintf(stderr, "----------xk = xk + ss*d_x--------\n");
xk.MatrInc(d_x);
// for (int i = 0; i < xk.GetRows(); i++)
// fprintf(stderr, "%.20f\n", xk(i,0));
d_lambda.MatrScale(ss);
lambda_k.MatrInc(d_lambda);
// fprintf(stderr, "-------lambda_k------\n");
// for (int i = 0; i < lambda_k.GetRows(); i++)
// fprintf(stderr, "%.20f\n", lambda_k(i,0));
sysd.ShurComplementProduct(grad_f, &xk, 0);
grad_f.MatrInc(opt_r);
evaluateConstraints(mfric.GetAddress(), numContacts, false);
computeSchurKKT(grad_f.GetAddress(), mfric.GetAddress(), numContacts, barrier_t, false);
// std::cout << "----r_dual-----" << std::endl;
// for (int i = 0; i < r_dual.GetRows(); i++)
// std::cout << r_dual(i,0) << std::endl;
// std::cout << "-----r_cent-----" << std::endl;
// for (int i = 0; i < r_cent.GetRows(); i++)
// std::cout << r_cent(i,0) << std::endl;
fprintf(stderr, "stage[%d], rd = %e, rg = %e, s = %f, t = %f\n", stage+1, r_dual.NormInf(), r_cent.NormInf(), ss, barrier_t);
if (r_cent.NormInf() < 1e-10 ||stage == (numStages - 1)){
fprintf(stderr, "solution found after %d stages!\n", stage+1);
// fprintf(stderr, "stage[%d], rd = %e, rg = %e, s = %f, t = %f\n", stage+1, r_dual.NormInf(), r_cent.NormInf(), ss, barrier_t);
delete [] BlockDiagonal_val;
delete [] BlockDiagonal_i;
delete [] BlockDiagonal_j;
delete [] spike_rhs;
/////////////////////////////////////////////
//set small-magnitude contact force to zero//
/////////////////////////////////////////////
// for (int i = 0; i < numContacts; i++){
// if (sqrt(pow(xk(3*i,0),2) + pow(xk(3*i+1,0),2) + pow(xk(3*i+2,0),2)) < 1e-6){
/// xk(3*i,0) = 0;
// xk(3*i+1,0) = 0;
// xk(3*i+2, 0) = 0;
// }
// }
sysd.FromVectorToConstraints(xk);
sysd.FromVectorToVariables(mq);
for (size_t ic = 0; ic < mconstraints.size(); ic ++){
if (mconstraints[ic]->IsActive())
mconstraints[ic]->Increment_q(mconstraints[ic]->Get_l_i());
}
// return r_cent.NormInf();
return 1e-8;
}
evaluateConstraints(mfric.GetAddress(), numContacts, false);
}
}
