example_main.cc

#include <stdlib.h>
#include <stdio.h>
#include "traj2dmp.h"
#include "optpart.h"
#include "cuttraj.h"
#include "file2mat.h"
#include <vector>
#include <netdb.h>
#include <string.h>
#include <sys/times.h>
#include <iostream>
#include "dmp.h"
#include <unistd.h>
#include <armadillo>


using namespace std;
using namespace arma;


int main(int argc, char *argv[]) {
	
	// Initialize original trajectories:
	double dt = 0.200; // 5 Hz sampling rate, in this example
	mat yDef;
	file2mat("yDef.txt",yDef);
	cout << yDef << endl;
	
	
	mat yCorr;
	//mat yCorr = file2mat("yCorr.txt");
	
	cout << yDef.n_rows << endl;
	//cout << yCorr.n_rows << endl;
	
	
	
	mat yDefKeep;  // kept part of deficient trajectory
	mat yCorrKeep; // kept part of corrective trajectory

	cuttraj(yDef, yCorr, yDefKeep, yCorrKeep); //Determine which part of trajectories to keep

	cout << "cuttraj done "  << endl;
	cout << yDefKeep  << endl;
	cout << yDef.n_rows  << endl;
	cout << yDefKeep.n_rows  << endl;
	// extrapolate/intrapolate to length 100 (for CVXGEN solver):
	vec x = linspace<vec>(0,1,yDefKeep.n_rows);
	cout << "hej "  << endl;
	mat xx = linspace<vec>(0,1,100);
	mat yDefKeepExtrap;
	cout << "hej "  << endl;
	mat yMod = zeros(100, yDefKeep.n_cols);
	// loop over dimensions j and solve optimization problem:
	cout << "extrap done "  << endl;
	for (int j = 0; j < yDef.n_cols; ++j) {
		interp1(x, yDefKeep.col(j), xx, yDefKeepExtrap);
		yMod.col(j) = optpart(yDefKeepExtrap, yCorrKeep.col(j));
	}
	cout << "optimization done "  << endl;

	Dmp resDmp1 = traj2dmp(yMod, dt*yMod.n_rows/yMod.n_rows).speedupTimes(1);


	Dmp resDmp2 = traj2dmp(yCorrKeep, dt).speedupTimes(1);

	cout << "First resulting DMP: " << endl << resDmp1 << endl;
	cout << "Second resulting DMP: " << endl << resDmp2 << endl;

}