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); } }