void DfHpqX::getHpq(TlDenseSymmetricMatrix_Lapack* pHpq, TlDenseSymmetricMatrix_Lapack* pHpq2) { const int numOfAOs = this->m_nNumOfAOs; // make coordinates const Fl_Geometry flGeom((*this->pPdfParam_)["coordinates"]); const int numOfAtoms = flGeom.getNumOfAtoms(); const int numOfDummyAtoms = flGeom.getNumOfDummyAtoms(); const int numOfRealAtoms = numOfAtoms - numOfDummyAtoms; std::vector<TlAtom> Cs(numOfRealAtoms); std::vector<TlAtom> Xs(numOfDummyAtoms); std::size_t realAtomIndex = 0; std::size_t dummyAtomIndex = 0; for (int i = 0; i < numOfAtoms; ++i) { const std::string atomName = flGeom.getAtomSymbol(i); const TlPosition p = flGeom.getCoordinate(i); const double charge = flGeom.getCharge(i); const TlAtom atom(atomName, p, charge); if (atomName == "X") { Xs[dummyAtomIndex] = atom; ++dummyAtomIndex; } else { Cs[realAtomIndex] = atom; ++realAtomIndex; } } assert(realAtomIndex == Cs.size()); assert(dummyAtomIndex == Xs.size()); pHpq->resize(numOfAOs); pHpq2->resize(numOfAOs); this->createEngines(); DfTaskCtrl* pTaskCtrl = this->getDfTaskCtrlObject(); std::vector<DfTaskCtrl::Task2> taskList; bool hasTask = pTaskCtrl->getQueue2(this->orbitalInfo_, true, this->grainSize_, &taskList, true); while (hasTask == true) { this->getHpq_part(this->orbitalInfo_, taskList, Cs, Xs, pHpq, pHpq2); hasTask = pTaskCtrl->getQueue2(this->orbitalInfo_, true, this->grainSize_, &taskList); } if (this->chargeExtrapolateNumber_ > 0) { *pHpq2 /= static_cast<double>(this->chargeExtrapolateNumber_); } this->finalize(pHpq, pHpq2); delete pTaskCtrl; pTaskCtrl = NULL; this->destroyEngines(); }
int estimatedAdderCost(const Linear& c) { // (sorry about strange implementation -- copy/paste programming) vec<Int> Cs(c.size); Int max_C = -1; for (int i = 0; i < c.size; i++){ Cs[i] = c(i); if (Cs[i] > max_C) max_C = Cs[i]; } int cost = 0; for (; max_C > 0; max_C >>= 1){ for (int i = 0; i < Cs.size(); i++){ if ((Cs[i] & 1) != 0) cost++; Cs[i] >>= 1; } } return cost; }
void addPb(const vec<Formula>& ps, const vec<Int>& Cs_, vec<Formula>& out, int bits) { assert(ps.size() == Cs_.size()); vec<vec<Formula> > pools; vec<Int> Cs(Cs_.size()); Int max_C = -1; for (int i = 0; i < Cs_.size(); i++){ Cs[i] = Cs_[i]; if (Cs[i] > max_C) max_C = Cs[i]; } for (; max_C > 0; max_C >>= 1){ pools.push(); for (int i = 0; i < Cs.size(); i++){ if ((Cs[i] & 1) != 0) pools.last().push(ps[i]); Cs[i] >>= 1; } } vec<Formula> carry; for (int p = 0; p < pools.size(); p++){ vec<Formula>& pool = pools[p]; carry.clear(); if (p == bits){ Formula overflow = _0_; for (; p < pools.size(); p++) for (int i = 0; i < pools[p].size(); i++) overflow |= pools[p][i]; out.push(overflow); }else if (pool.size() == 0) out.push(_0_); else{ int head = 0; while (pool.size()-head >= 3){ pool .push(FAs(pool[head], pool[head+1], pool[head+2])); carry.push(FAc(pool[head], pool[head+1], pool[head+2])); head += 3; } if (pool.size()-head == 2){ pool .push(FAs(pool[head], pool[head+1], _0_)); carry.push(FAc(pool[head], pool[head+1], _0_)); head += 2; } assert(pool.size()-head == 1); out.push(pool[head]); } if (carry.size() > 0){ if (p+1 == pools.size()) pools.push(); for (int i = 0; i < carry.size(); i++) pools[p+1].push(carry[i]); } } #if 0 //DEBUG for (int p = 0; p < pools.size(); p++){ printf("pool %d:", (1 << p)); for (int i = 0; i < pools[p].size(); i++){ printf(" "); dump(pools[p][i]); } printf("\n"); } #endif }
int main(int argc, char** argv) { int rv = MPI_Init(&argc, &argv); assert(rv == MPI_SUCCESS); int size; rv = MPI_Comm_size(MPI_COMM_WORLD, &size); assert(rv == MPI_SUCCESS); int rank; rv = MPI_Comm_rank(MPI_COMM_WORLD, &rank); assert(rv == MPI_SUCCESS); MPI_Status status; assert(size == 2); assert(M % 2 == 0); double start_time = MPI_Wtime(); std::vector<Value> A(M + 1); std::vector<Value> B(M + 1); std::vector<Value> C(M + 1); std::vector<Value> F(M + 1); A[0] = Value(0.0, 0.0); // Not used. for (int m = 1; m <= M - 1; m++) { A[m] = Value(d / h / h, D / h / h); } A[M] = Value(1.0, 1.0); B[0] = Value(1.0, 1.0); for (int m = 1; m <= M - 1; m++) { B[m] = Value(-2.0 * d / h / h - 1.0 / tau, -2.0 * D / h / h - 1.0 / tau); } B[M] = Value(-1.0, -1.0); C[0] = Value(-1.0, -1.0); for (int m = 1; m <= M - 1; m++) { C[m] = Value(d / h / h, D / h / h); } C[M] = Value(0.0, 0.0); // Not used. std::vector<Value> As(M + 1); std::vector<Value> Bs(M + 1); std::vector<Value> Cs(M + 1); std::vector<Value> Fs(M + 1); As[0] = Value(0.0, 0.0); for (int m = 1; m <= M; m++) { As[m] = -A[m] * A[m - 1] / B[m - 1]; } Bs[0] = B[0] - C[0] * A[1] / B[1]; for (int m = 1; m <= M - 1; m++) { Bs[m] = B[m] - A[m] * C[m - 1] / B[m - 1] - C[m] * A[m + 1] / B[m + 1]; } Bs[M] = B[M] - A[M] * C[M - 1] / B[M - 1]; for (int m = 0; m <= M - 1; m++) { Cs[m] = -C[m] * C[m + 1] / B[m + 1]; } Cs[M] = Value(0.0, 0.0); std::vector<Value> prev_values(M + 1); std::vector<Value> curr_values(M + 1); InitializeValues(curr_values); std::vector<Value> P(M + 1); std::vector<Value> Q(M + 1); for (int n = 0; n < N; n++) { prev_values.swap(curr_values); F[0] = Value(0.0, 0.0); for (int m = 1; m <= M - 1; m++) { if (m % 2 == rank) { F[m].u = -prev_values[m].u / tau - f(prev_values[m]); F[m].v = -prev_values[m].v / tau - g(prev_values[m]); } } F[M] = Value(0.0, 0.0); for (int m = 1; m <= M - 1; m++) { if (m % 2 == rank) { rv = MPI_Send(&F[m].u, 1, MPI_DOUBLE, (rank + 1) % 2, m, MPI_COMM_WORLD); assert(rv == MPI_SUCCESS); rv = MPI_Send(&F[m].v, 1, MPI_DOUBLE, (rank + 1) % 2, m, MPI_COMM_WORLD); assert(rv == MPI_SUCCESS); } else { rv = MPI_Recv(&F[m].u, 1, MPI_DOUBLE, (rank + 1) % 2, m, MPI_COMM_WORLD, &status); assert(rv == MPI_SUCCESS); rv = MPI_Recv(&F[m].v, 1, MPI_DOUBLE, (rank + 1) % 2, m, MPI_COMM_WORLD, &status); assert(rv == MPI_SUCCESS); } } Fs[0] = F[0] - C[0] / B[1] * F[1]; for (int m = 1; m <= M - 1; m++) { if (m % 2 == rank) { Fs[m] = F[m] - A[m] / B[m - 1] * F[m - 1] - C[m] / B[m + 1] * F[m + 1]; } } Fs[M] = F[M] - A[M] / B[M - 1] * F[M - 1]; if (rank == 0) { P[0] = -Cs[0] / Bs[0]; Q[0] = Fs[0] / Bs[0]; } else { P[1] = -Cs[1] / Bs[1]; Q[1] = Fs[1] / Bs[1]; } for (int m = 2; m <= M; m++) { if (m % 2 == rank) { P[m] = -Cs[m] / (As[m] * P[m - 2] + Bs[m]); Q[m] = (Fs[m] - As[m] * Q[m - 2]) / (As[m] * P[m - 2] + Bs[m]); } } if (M % 2 == rank) { curr_values[M] = Q[M]; } else { curr_values[M - 1] = Q[M - 1]; } for (int m = M - 2; m >= 0; m--) { if (m % 2 == rank) { curr_values[m] = P[m] * curr_values[m + 2] + Q[m]; } } } for (int m = 0; m <= M; m++) { if (m % 2 == rank) { rv = MPI_Send(&curr_values[m].u, 1, MPI_DOUBLE, (rank + 1) % 2, m, MPI_COMM_WORLD); assert(rv == MPI_SUCCESS); rv = MPI_Send(&curr_values[m].v, 1, MPI_DOUBLE, (rank + 1) % 2, m, MPI_COMM_WORLD); assert(rv == MPI_SUCCESS); } else { rv = MPI_Recv(&curr_values[m].u, 1, MPI_DOUBLE, (rank + 1) % 2, m, MPI_COMM_WORLD, &status); assert(rv == MPI_SUCCESS); rv = MPI_Recv(&curr_values[m].v, 1, MPI_DOUBLE, (rank + 1) % 2, m, MPI_COMM_WORLD, &status); assert(rv == MPI_SUCCESS); } } if (rank == 0) { printf("%lf\n", MPI_Wtime() - start_time); /* for (int m = 0; m < M; m++) { double coord = X * m / M; printf("%lf %lf %lf\n", coord, curr_values[m].u, curr_values[m].v); } */ } rv = MPI_Finalize(); assert(rv == MPI_SUCCESS); return EXIT_SUCCESS; }
void DfHpqX::getForce_partProc(const TlOrbitalInfoObject& orbitalInfo, const int shellTypeP, const int shellTypeQ, const index_type shellIndexP, const ShellArray& shellArrayQ, const TlMatrixObject& P, TlDenseGeneralMatrix_Lapack* pForce_woX, TlDenseGeneralMatrix_Lapack* pForce_Xonly) { static const int BUFFER_SIZE_NUC = 3 * 5 * 5 * 5; // (xyz) * 5d * 5d * 5d const Fl_Geometry flGeom((*this->pPdfParam_)["coordinates"]); const int maxStepsP = 2 * shellTypeP + 1; const int maxStepsQ = 2 * shellTypeQ + 1; const DfHpqEngine::Query queryPQ10(1, 0, shellTypeP, shellTypeQ); const DfHpqEngine::Query queryPQ01(0, 1, shellTypeP, shellTypeQ); const TlPosition posP = orbitalInfo_.getPosition(shellIndexP); const index_type atomIndexA = orbitalInfo_.getAtomIndex(shellIndexP); const int numOfAtoms = this->m_nNumOfAtoms; const DfHpqEngine::PGTOs pgtosP = this->getPGTOs(shellIndexP); #pragma omp parallel { int threadID = 0; #ifdef _OPENMP threadID = omp_get_thread_num(); #endif // _OPENMP double* p_dNuc_dA = new double[BUFFER_SIZE_NUC]; double* p_dNuc_dB = new double[BUFFER_SIZE_NUC]; const std::size_t shellArraySizeQ = shellArrayQ.size(); #pragma omp for schedule(runtime) for (std::size_t q = 0; q < shellArraySizeQ; ++q) { const index_type shellIndexQ = shellArrayQ[q]; const TlPosition posQ = orbitalInfo.getPosition(shellIndexQ); const DfHpqEngine::PGTOs pgtosQ = this->getPGTOs(shellIndexQ); const index_type atomIndexB = orbitalInfo.getAtomIndex(shellIndexQ); // 重複ペアの排除 // if (shellIndexP < shellIndexQ) { // continue; // } // 運動エネルギー this->pEngines_[threadID].calcKineticPart(queryPQ10, posP, posQ, pgtosP, pgtosQ); { int index = 0; for (int stepP = 0; stepP < maxStepsP; ++stepP) { const index_type indexP = shellIndexP + stepP; for (int stepQ = 0; stepQ < maxStepsQ; ++stepQ) { const index_type indexQ = shellIndexQ + stepQ; double coef = P.get(indexP, indexQ); const double dKin_dA = this->pEngines_[threadID].WORK_KIN[index]; const double dKin_dB = -dKin_dA; #pragma omp critical(DfHpqX__getForce_partProc) { pForce_woX->add(atomIndexA, X, coef * dKin_dA); pForce_woX->add(atomIndexB, X, coef * dKin_dB); } ++index; } } for (int stepP = 0; stepP < maxStepsP; ++stepP) { const index_type indexP = shellIndexP + stepP; for (int stepQ = 0; stepQ < maxStepsQ; ++stepQ) { const index_type indexQ = shellIndexQ + stepQ; double coef = P.get(indexP, indexQ); const double dKin_dA = this->pEngines_[threadID].WORK_KIN[index]; const double dKin_dB = -dKin_dA; #pragma omp critical(DfHpqX__getForce_partProc) { pForce_woX->add(atomIndexA, Y, coef * dKin_dA); pForce_woX->add(atomIndexB, Y, coef * dKin_dB); } ++index; } } for (int stepP = 0; stepP < maxStepsP; ++stepP) { const index_type indexP = shellIndexP + stepP; for (int stepQ = 0; stepQ < maxStepsQ; ++stepQ) { const index_type indexQ = shellIndexQ + stepQ; double coef = P.get(indexP, indexQ); const double dKin_dA = this->pEngines_[threadID].WORK_KIN[index]; const double dKin_dB = -dKin_dA; #pragma omp critical(DfHpqX__getForce_partProc) { pForce_woX->add(atomIndexA, Z, coef * dKin_dA); pForce_woX->add(atomIndexB, Z, coef * dKin_dB); } ++index; } } } // 核-電子反発 for (index_type atomIndexC = 0; atomIndexC < numOfAtoms; ++atomIndexC) { const std::string atomName = flGeom.getAtomSymbol(atomIndexC); const TlPosition p = flGeom.getCoordinate(atomIndexC); const double charge = flGeom.getCharge(atomIndexC); const TlAtom C(atomName, p, charge); std::vector<TlAtom> Cs(1); Cs[0] = C; this->pEngines_[threadID].calcNuclearAttractionPart( queryPQ10, posP, posQ, pgtosP, pgtosQ, Cs); std::copy(this->pEngines_[threadID].WORK_NUC, this->pEngines_[threadID].WORK_NUC + BUFFER_SIZE_NUC, p_dNuc_dA); this->pEngines_[threadID].calcNuclearAttractionPart( queryPQ01, posP, posQ, pgtosP, pgtosQ, Cs); std::copy(this->pEngines_[threadID].WORK_NUC, this->pEngines_[threadID].WORK_NUC + BUFFER_SIZE_NUC, p_dNuc_dB); TlDenseGeneralMatrix_Lapack* pForce = pForce_woX; if (C.getSymbol() == "X") { pForce = pForce_Xonly; } int index = 0; for (int stepP = 0; stepP < maxStepsP; ++stepP) { const index_type indexP = shellIndexP + stepP; for (int stepQ = 0; stepQ < maxStepsQ; ++stepQ) { const index_type indexQ = shellIndexQ + stepQ; double coef = P.get(indexP, indexQ); const double gradA = p_dNuc_dA[index]; const double gradB = p_dNuc_dB[index]; const double gradC = -(gradA + gradB); #pragma omp critical(DfHpqX__getForce_partProc) { pForce->add(atomIndexA, X, coef * gradA); pForce->add(atomIndexB, X, coef * gradB); pForce->add(atomIndexC, X, coef * gradC); } ++index; } } for (int stepP = 0; stepP < maxStepsP; ++stepP) { const index_type indexP = shellIndexP + stepP; for (int stepQ = 0; stepQ < maxStepsQ; ++stepQ) { const index_type indexQ = shellIndexQ + stepQ; double coef = P.get(indexP, indexQ); const double gradA = p_dNuc_dA[index]; const double gradB = p_dNuc_dB[index]; const double gradC = -(gradA + gradB); #pragma omp critical(DfHpqX__getForce_partProc) { pForce->add(atomIndexA, Y, coef * gradA); pForce->add(atomIndexB, Y, coef * gradB); pForce->add(atomIndexC, Y, coef * gradC); } ++index; } } for (int stepP = 0; stepP < maxStepsP; ++stepP) { const index_type indexP = shellIndexP + stepP; for (int stepQ = 0; stepQ < maxStepsQ; ++stepQ) { const index_type indexQ = shellIndexQ + stepQ; double coef = P.get(indexP, indexQ); const double gradA = p_dNuc_dA[index]; const double gradB = p_dNuc_dB[index]; const double gradC = -(gradA + gradB); #pragma omp critical(DfHpqX__getForce_partProc) { pForce->add(atomIndexA, Z, coef * gradA); pForce->add(atomIndexB, Z, coef * gradB); pForce->add(atomIndexC, Z, coef * gradC); } ++index; } } } } delete[] p_dNuc_dA; p_dNuc_dA = NULL; delete[] p_dNuc_dB; p_dNuc_dB = NULL; } }
int main(int argc, const char *argv[]) { options::single<int> number('N', "number", "some number", 0); options::single<int> number2('k', "kumber", "some other number", 0, {0, 3, 3}); options::map<int, std::vector<std::pair<std::string, int> > > numbers('m', "numbers", "several numbers"); options::map<std::string, std::list<std::pair<std::string, std::string> > > strings('s', "strings", "several strings"); options::single<const char *>cs('c', "cstring", "a c string", "", {"bla", "blubb"}); options::single<std::string>Cs('C', "Cstring", "a C++ string", "", {"bla", "blubb"}); options::container<double> dnums('d', "doubles", "double numbers"); // options::container<const char*, std::list<const char*>> stringl('S', "listString", "list of strings"); options::container<std::string, std::list<std::string>> stringS('X', "liststring", "list of std::strings"); options::single<double> complexDescription('\0', "ComplexDescription", "Pass here the Bremsstrahl-Tagging-Hodoscope-Engineering-Assemply-Rate in Hz", 84.); options::single<double> moreComplexDescription('\0', "MoreComplexDescription", "very complicated example option with very long explanation to illustrate automatic wrapping in help output when the explanations become very long and would break readability otherwise.", 42.); options::single<double> evenMoreComplexDescription('\0', "EvenMoreComplexDescription", "very complicated example option with very long explanation containing averylongwordwhichisunbreakableandthustriggersforcefulwordwrappinginaninconvenientplacetokeepthingssomehowatleastabitreadable.", 21.); options::single<options::postFixedNumber<size_t>> size('\0', "size", "a size"); options::single<std::chrono::system_clock::time_point> date('\0', "date", "a date"); options::single<std::chrono::duration<long>> dur('\0', "dur", "a duration"); options::single<bool> lateOption('\0', "lateOption", "try to book an option late", false); cs.fForbid(&Cs); Cs.fForbid(&cs); options::container<std::chrono::system_clock::time_point> dates('\0', "dates", "list of dates"); options::OptionsForTApplication TApplicationOptions(argv[0]); options::positional<options::single<float>>posNumber(10, "posnumber", "positional float number", 0); options::positional<options::container<std::string>>files(20, "files", "positional file list"); options::positional<options::single<const char *>>dest(30, "dest", "positional destination file", ""); options::parser parser("option parsing example"); parser.fRequire(&number); auto unusedOptions = parser.fParse(argc, argv); TApplicationOptions.fFinalize(unusedOptions.begin(), unusedOptions.end()); for (auto & unusedOption : unusedOptions) { std::cout << "unused option : '" << unusedOption << "'" << std::endl; } for (auto & num : numbers) { std::cout << " number '" << num.first << "' is '" << num.second << "'\n"; } for (auto & str : strings) { std::cout << " string '" << str.first << "' is '" << str.second << "'\n"; } for (double & dnum : dnums) { std::cout << " double number '" << dnum << "'\n"; } // for (auto & it : stringl) { // std::cout << " list string '" << it << "'\n"; //} for (auto & it : stringS) { std::cout << " list std::string '" << it << "'\n"; } std::cout << "and the time variable is:"; date.fWriteValue(std::cout); auto timebuf = std::chrono::system_clock::to_time_t(date); std::cout << ", that is " << std::ctime(&timebuf); std::cout << "the duration is: "; dur.fWriteValue(std::cout); std::cout << " or " << std::chrono::duration_cast<std::chrono::hours>(dur).count() << " hours \n"; if (lateOption) { options::single<bool> optionLate('\0', "lateOptionTest", "option booked late", false); } return number; }
int main(int argc, char** argv) { // Check for trajectory names if(argv[1] == NULL) { std::cout << "Missing file name [Trajectory]" << std::endl; return 1; } else if (argc < 5) { std::cerr << "Wrong number of parameters!!" << std::endl; std::cerr << "Usage: ./iri_learnProMpLocal <trajectory_params_name> <cartesian_trajectories_path> <number_of_demos> <number_of_gaussians>" << std::endl; return -1; } std::string trajectory_filename(argv[1]); std::string cartesianpath(argv[2]); int Ndemos=atoi(argv[3]); int Nf = atoi(argv[4]); std::cout << "Trajectory name: " << trajectory_filename << std::endl; std::cout << "Casrtesian trajectory files path: " << cartesianpath << std::endl; std::cout << "Number of demos: " << Ndemos << std::endl; std::cout << "Number of gaussians: " << Nf << std::endl; //Compensate Gravity! /*std::cout << "Enter the number of trajectories that will generate the ProMP = "; //waitForEnter(); int Ndemos; int foo; //std::cin>>Ndemos; std::string line; std::getline(std::cin, line);*/ //Ndemos=atoi(line.c_str()); Eigen::MatrixXd JP, JP2, JV, JV2, Y0, JPout, JVout, Tout, T0; Eigen::VectorXd w; //std::vector<Eigen::MatrixXd> Demos_JP; //std::vector<Eigen::MatrixXd> Demos_JV; int solved, Nred; Nred = 1; /*// GET DATA FROM DEMONSTRATIONS for (int i=0; i<Ndemos; i++){ std::cout << "Press enter to START recording a desired trajectory (first move the robot to the initial position)" << std::endl; std::getline(std::cin, line); std::cout << "started" << std::endl; traj_rec.start(); my_ramp.start(); // teach motion to robot std::cout << "Press enter to STOP recording the desired trajectory" << std::endl; std::getline(std::cin, line); std::cout << "stopped" << std::endl << std::endl; traj_rec.stop(); my_ramp.stop(); JP = traj_rec.GetPosMatrix(); JV = traj_rec.GetVelMatrix(); ac_math::ExtractDataExperiment(JP, JV, timeratio, JPout, JVout, Tout); if(i==0){ T0 = Tout; Y0 = JPout; Demos_JP.push_back(JPout); Demos_JV.push_back(JVout); ac_math::SaveMatrixAsFile(JPout,"test",i); }else{ ac_math::SaveMatrixAsFile(JPout,"test",i); // std::cout<<"\n JPout=\n "<<JPout; solved=ac_math::IterativeTimeWarping(Y0,JPout,w,JP2,timeratio); // if(solved==0){ // Nred=1; // std::cout<<"Didn't work, recomputing"<<endl; // solved=ac_math::IterativeTimeWarping(Y0,JPout,w,JP2,Nred,timeratio); // } ac_math::SaveMatrixAsFile(JP2,"shifted",i); Demos_JP.push_back(JP2); } } std::cout << "Time warping completed. Trajectories stored." << std::endl;*/ ////Eigen::MatrixXd Jpaux; // ProMP parameters // define ProMP centers and widths, we set final_time=1 and we'll rescale it later. Eigen::VectorXd Cs(Nf); for(int s=0; s<Nf; s++) { Cs(s)=((double) s)/((double) Nf); } double h_s; h_s = 1.0/(4.0f*double(Nf)); Eigen::MatrixXd weights_aux, weights, DATAcart; int cartesiancontrol, d; //std::cout << "Do you want to store trajectory as a cartesian trajectory (press 1 and enter) or joint trajectory (press 0 and enter)" << std::endl; //std::cin >> cartesiancontrol; cartesiancontrol = 1; // FIXME note this as the test just was using cartesian if(cartesiancontrol==0) { std::cout<<"\n You chose joint trajectory"; d=7; weights.resize(Nf*d, Ndemos); weights.fill(0.0); } else { std::cout<<"\n You chose cartesian trajectory"; // for the cartesian version d=6; weights.resize(Nf*d,Ndemos); weights.fill(0.0); } std::cout << std::endl; //WAMIK WAM_kinematics; /*Eigen::VectorXd q_aux(7),u_aux,u_aux_old; u_aux.resize(3); u_aux.fill(0.0); u_aux_old.resize(3); u_aux_old.fill(0.0); Eigen::MatrixXd T_aux, R_aux; R_aux.resize(3,3);*/ //usleep(5000000); /*Eigen::VectorXd Y0mean; Y0mean.resize(7); Y0mean.fill(0.0);*/ for(int k=0; k<Ndemos; k++){ // compute weights of k-th trajectory /*Jpaux=Demos_JP[k]; std::cout << "CHECK 1, k= " << k << std::endl; for(int joi=0;joi<7;joi++){ Y0mean(joi)=Y0mean(joi)+Jpaux(0,joi)/Ndemos; }*/ /*if (cartesiancontrol==0){ // control in joints, we only consider joint space trajectory weights_aux=ac_math::FitParamsFromTraj(Cs,h_s,Jpaux); weights.block(0,k,Nf*d,1) = weights_aux.block(0,0,Nf*d,1); // every column is one experiment. }else{ //control in cartesian space, we need their cartesian expression. /*std::cout << "CHECK 2" << std::endl; DATAcart.resize(Jpaux.rows(),d); for(int i=0; i<Jpaux.rows(); i++){ // convert Jpaux.row(i) to cartesian trajectory (Homogeneous transformation) with forward kinematics for(int j=0; j<7; j++){ q_aux(j)=Jpaux(i,j); } std::cout << "CHECK 3" << std::endl; T_aux=WAM_kinematics.forwardkinematics(q_aux); std::cout << "CHECK 4" << std::endl; // convert homogeneous transformation to state vector R_aux.block(0,0,3,3)=T_aux.block(0,0,3,3); u_aux_old=u_aux; u_aux=ac_math::Quat2TriVec(ac_math::Rot2Quat(R_aux)); std::cout << "CHECK 5" << std::endl; double v1,v2; std::cout << "u_aux= " << u_aux << std::endl; std::cout << "u_aux2= " << u_aux_old << std::endl; v1=(std::abs(u_aux_old(0)-u_aux(0))+std::abs(u_aux_old(1)-u_aux(1))+std::abs(u_aux_old(2)-u_aux(2))); std::cout << "CHECK 5a" << std::endl; v2=(std::abs(u_aux_old(0)+u_aux(0))+std::abs(u_aux_old(1)+u_aux(1))+std::abs(u_aux_old(2)+u_aux(2))); std::cout << "CHECK 5b" << std::endl; if (i>1 && v1>v2+0.01){ u_aux=-u_aux; u_aux_old=u_aux; std::cout << "Changed sign in k,i= " << k << ", " << i << std::endl; } std::cout << "CHECK 6" << std::endl; //u_aux=ac_math::Rot2Quat(R_aux); // store in DATAcart DATAcart(i,0) = T_aux(0,3);// x coord DATAcart(i,1) = T_aux(1,3);// y coord DATAcart(i,2) = T_aux(2,3);// z coord DATAcart(i,3) = u_aux(0);// rx coord DATAcart(i,4) = u_aux(1);// ry coord DATAcart(i,5) = u_aux(2);// rz coord // DATAcart(i,6)=u_aux(3);// rz coord std::cout << "CHECK 7" << std::endl; } std::cout << "CHECK 8" << std::endl;*/ // GCC: Count rows and columns and load data std::string cartesianfname = cartesianpath + "/Cartesian" + std::to_string(k); int rows = 0; std::ifstream in(cartesianfname + ".txt"); if (!in.is_open()) { std::cerr << "Error: Cartesian traj file \"" << cartesianfname << ".txt\" was not found." << std::endl; return -1; } std::string unused; std::getline(in, unused); int cols = std::count(unused.begin(), unused.end(), ',') + 1; ++rows; while (std::getline(in, unused)) ++rows; std::cout << cartesianfname << " -- rows: " << rows << " cols: " << cols << " -- " << unused << std::endl; DATAcart = ac_math::LoadDataMatrix(cartesianfname, rows, cols); // GCC weights_aux = ac_math::FitParamsFromTraj(Cs,h_s,DATAcart); weights.block(0,k,Nf*d,1) = weights_aux.block(0,0,Nf*d,1); //ac_math::SaveMatrixAsFile(DATAcart,"Cartesian",k); //} } std::cout << "Weights = " << weights << std::endl; // Fit parameters with all the weights Eigen::MatrixXd Sw,mw; ac_math::FitNormalDistribution(weights,Sw,mw); // STORE PROMP DATA AS FILES std::string SfileName(trajectory_filename), mfileName(trajectory_filename); SfileName += "Sw"; mfileName += "mw"; ac_math::SaveMatrixAsFile(Sw, SfileName.c_str(), -1.0); ac_math::SaveMatrixAsFile(mw, mfileName.c_str(), -1.0); // Save other things /*std::ofstream OFile; std::string OfileName(trajectory_filename); OfileName += "O.txt"; OFile.open(OfileName.c_str()); // write C OFile << Cs(0); for (int s=1;s<Nf;s++){ OFile << "," << Cs(s); } OFile << "\n"; // write Nf OFile << Nf; OFile << "\n"; // write time OFile << ((double)Y0.rows())*0.002*((double)timeratio); OFile << "\n"; // write width OFile << h_s; OFile << "\n"; // write dimension OFile << d; OFile << "\n"; // write initial Joint position OFile << Y0mean(0); for (int jo=1;jo<7;jo++){ OFile << "," << Y0mean(jo); } OFile << "\n"; // close file OFile.close();*/ return 0; }