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