Commit 6876d7fc authored by Martin Karlsson's avatar Martin Karlsson
Browse files

lots of changes

parent a61baadf
CC=g++
CFLAGS=-I. -Wall \
-fpermissive -lpthread -std=c++11 -I~/.progs/armadillo-6.400.3/include -DARMA_DONT_USE_WRAPPER -larmadillo #-llapack -lblas
CFLAGS = -g -O2 -Wall -W -pedantic-errors
# Change the following to include current directory to armadillo.../include:
CFLAGS += -Wmissing-braces -Wparentheses -Wold-style-cast -I/home/martinka/miscellaneous/armadillo-6.400.3/include -DARMA_DONT_USE_WRAPPER -lblas -llapack
CFLAGS += -std=c++11
LFLAGS=-lrt
#LIBS=-lm -llapack -lblas
OBJSTATIC = traj2dmp.o file2mat.o\
optpart.o solver.o cvxgen/matrix_support.o cvxgen/util.o cvxgen/ldl.o cuttraj.o dmp.o
LIBS=-lm -llapack -lblas
OBJSTATIC = traj2dmp.o plotMat3.o plotgnu3.o\
optpart.o cvxgen/solver.o cvxgen/matrix_support.o cvxgen/util.o cvxgen/ldl.o cuttraj.o dmp.o
all: example_main
%: %.c
$(CC) -o $@ $(CFLAGS) $^ $(LIBS)
%.o: %.cpp $(DEPS)
%.o: %.cc $(DEPS)
$(CC) $(CFLAGS) -c -o $@ $<
......
......@@ -33,7 +33,6 @@ void cuttraj(const mat& evalTraj, const mat& corrTraj, mat& traj1, mat& traj2) {
mat dist2cutnew = dists.col(cutnew);
double mindist2cutnew = as_scalar(min(dist2cutnew));
cout << mindist2cutnew << endl;
double cutold = as_scalar(find(dist2cutnew == mindist2cutnew, 1, "first")) + 1;
......
......@@ -4,7 +4,19 @@
using namespace arma;
using namespace std;
Dmp::Dmp(mat w, mat g, double t) : weights(w), goal(g), tau(t) {}
Dmp::Dmp(mat w, mat g, double t) : weights(w), goal(g), tau(t) {
nDims = w.n_cols;
x = ones<mat>(nDims,1);
v = zeros<mat>(nDims,1);
z = zeros<mat>(nDims,1);
int n_rfs = w.n_rows; // number of weight functions
mat arr = linspace<mat>(0, 1, n_rfs)*0.5;
c = (1+alpha_z/2*arr)%exp(-alpha_z/2*arr);
D = pow((diff(c)*0.55),2);
mat Dlast(1,1);
Dlast = D(D.n_rows - 1);
D = 1/join_vert(D, Dlast);
}
mat Dmp::getW() const {
return weights;
......@@ -18,8 +30,8 @@ double Dmp::getT() const {
return tau;
}
void Dmp::setG(mat g) {
goal = g;
void Dmp::setG(mat gIn) {
goal = gIn;
}
void Dmp::setT(double t) {
......@@ -27,12 +39,12 @@ void Dmp::setT(double t) {
}
Dmp& Dmp::doubleSpeed() {
tau = tau/2;
tau = 2*tau;
return *this;
}
Dmp& Dmp::speedupTimes(int x) {
tau = tau/x;
tau = x*tau;
return *this;
}
......@@ -46,3 +58,68 @@ ostream& operator<<(ostream& os, const Dmp& dmp) {
return os;
}
void Dmp::resetState() {
firstSample = true;
x = ones<mat>(nDims,1);
v = zeros<mat>(nDims,1);
z = zeros<mat>(nDims,1);
g = zeros<mat>(nDims,1);
}
mat Dmp::getVel(const mat& posIn, const double& dt) {
mat pos = posIn.t();
if (firstSample) {
g = pos;
firstSample = false;
}
mat vel = zeros(nDims,1);
for (int j = 0; j < nDims; ++j) {
//mat psi = zeros(d.n_rows, d.n_cols);
mat psi = exp(-0.5*pow((x(j)-c),2) % D);
double f = as_scalar(sum(v(j)*weights.col(j)%psi)/sum(psi+1.e-10));
// Differential equations
double vd = (alpha_v*(beta_v*(0-x(j))-v(j)))*tau;
double xd = v(j)*tau;
double zd = (alpha_z*(beta_z*(g(j)-pos(j))-z(j))+f)*tau;
double yd = z(j)*tau;
double gd = alpha_g*(goal(j)-g(j));
// State update:
x(j) = xd*dt+x(j);
v(j) = vd*dt+v(j);
z(j) = zd*dt+z(j);
g(j) = gd*dt+g(j);
vel(j) = yd;
}
return vel.t();
}
/*
for j = 1:7
psi = exp(-0.5*((x(j)-c).^2).*D);
f = sum(v(j)*w(:,j).*psi)/sum(psi+1.e-10);
% The dynamical systems:
vd = (alpha_v*(beta_v*(0-x(j))-v(j)))*tau;
xd = v(j)*tau;
zd = (alpha_z*(beta_z*(g(j)-yact(j))-z(j))+f)*tau;
yd = z(j)*tau;
gd = alpha_g*(G(j)-g(j));
% State update:
x(j) = xd*dt+x(j);
v(j) = vd*dt+v(j);
z(j) = zd*dt+z(j);
g(j) = gd*dt+g(j);
yrefd(j) = yd;
end
*/
......@@ -17,11 +17,12 @@ public:
void setT(double t);
Dmp& doubleSpeed();
Dmp& speedupTimes(int x);
mat getVel(const mat& pos, const double& dt);
void resetState();
private:
mat weights;
mat goal;
double tau;
//new, to be integrated:
string ID;
string GroupID;
......@@ -38,6 +39,21 @@ private:
string startTolerance;
string endTolerance;
string goalObject;
////////////////////////////////////////////
int nDims;
bool firstSample = true;
mat x;
mat v;
mat z;
mat g;
mat c;
mat D;
double alpha_g = 12.5;
double alpha_v = 25;
double alpha_z = 25;
double beta_v = 6.25;
//double beta_z = 6.25;
double beta_z = 8;
};
ostream& operator<<(ostream& os, const Dmp& dmp);
......
No preview for this file type
......@@ -3,7 +3,6 @@
#include "traj2dmp.h"
#include "optpart.h"
#include "cuttraj.h"
#include "file2mat.h"
#include <vector>
#include <netdb.h>
#include <string.h>
......@@ -12,6 +11,8 @@
#include "dmp.h"
#include <unistd.h>
#include <armadillo>
#include "plotMat3.h"
using namespace std;
......@@ -21,23 +22,12 @@ 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;
//cout << yDef.n_rows << endl;
cout << "hej" << endl;
mat yDef = ones<mat>(10,3);
cout << "hej" << endl;
mat yCorr = ones<mat>(10,3);
yCorr = yCorr*2;
//mat yCorr = file2mat("yCorr.txt");
cout << "hej" << endl;
cout << yCorr.n_rows << endl;
double dt = 0.040; // 25 Hz sampling rate, in this example
mat yDef;
yDef.load("yDef.txt", raw_ascii);
mat yCorr;
yCorr.load("yCorr.txt", raw_ascii);
int nDims = yDef.n_cols; // Number of trajectory dimensions
mat yDefKeep; // kept part of deficient trajectory
mat yCorrKeep; // kept part of corrective trajectory
......@@ -45,30 +35,56 @@ int main(int argc, char *argv[]) {
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);
mat yMod = zeros(100, nDims);
// loop over dimensions j and solve optimization problem:
cout << "extrap done " << endl;
for (int j = 0; j < yDef.n_cols; ++j) {
for (int j = 0; j < nDims; ++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);
cout << "optimization done " << endl;
Dmp resDmp1 = traj2dmp(yMod, dt*yDefKeep.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;
int simSamples1 = 500;
mat yRes1 = yDef.row(0);
mat vel = zeros(1,nDims);
mat yResNext = zeros(1,nDims);
for (int i = 0; i < simSamples1; ++i) { // Simulate trajectory from resDmp1
vel = resDmp1.getVel(yRes1.row(yRes1.n_rows-1), dt);
for (int j = 0; j < nDims; ++j) {
yResNext(j) = yRes1(yRes1.n_rows-1,j) + vel(j) * dt;
}
yRes1 = join_vert(yRes1, yResNext);
}
mat yRes2 = yCorrKeep.row(0);
for (int i = 0; i < simSamples1; ++i) { // Simulate trajectory from resDmp2
vel = resDmp2.getVel(yRes2.row(yRes2.n_rows-1), dt);
for (int j = 0; j < nDims; ++j) {
yResNext(j) = yRes2(yRes2.n_rows-1,j) + vel(j) * dt;
}
yRes2 = join_vert(yRes2, yResNext);
}
cout << "Start plotting" << endl;
plotMat3(yDef, "Deficient trajectory");
plotMat3(yCorr, "Corrective trajectory");
plotMat3(yMod, "Modified trajectory");
plotMat3(yRes1, "Resulting trajectory part 1");
plotMat3(yRes2, "Resulting trajectory part 2");
}
#include "cuttraj.h"
#include <iostream>
#include <armadillo>
using namespace arma;
using namespace std;
void file2mat(const string& filename, mat& y) {
mat yTemp;
yTemp.load(filename, raw_ascii);
y = zeros<mat>(yTemp.n_rows, yTemp.n_cols);
double dTemp;
for (int i = 0; i < y.n_rows; ++i) {
for (int j = 0; j < y.n_cols; ++j) {
dTemp = yTemp(i,j);
y(i,j) = dTemp;
}
}
cout << y.n_rows << endl;
cout << y.n_cols << endl;
y = ones<mat>(yTemp.n_rows, yTemp.n_cols);
}
#include <armadillo>
using namespace arma;
#ifndef FILE2MAT_H
#define FILE2MAT_H
using namespace std;
using namespace arma;
void file2mat(const string& filename, mat& y);
#endif
......@@ -6,7 +6,7 @@
/* Description: Basic test harness for solver.c. */
#include "solver.h"
#include "cvxgen/solver.h"
#include "optpart.h"
#include <iostream>
#include <armadillo>
......@@ -19,16 +19,12 @@ Settings settings;
mat optpart(const mat& traj1, const mat& traj2) {
set_defaults();
setup_indexing();
load_default_data(traj1, traj2);
settings.verbose = 1;
set_defaults();
setup_indexing();
load_default_data(traj1, traj2);
solve();
mat restraj = zeros<mat>(100,1);
for (int i = 1; i < 101; ++i) {
//cout << *vars.x[i] << endl;
restraj(i-1) = *vars.x[i];
}
return restraj;
......@@ -37,8 +33,7 @@ void load_default_data(const mat& traj1, const mat& traj2) {
for (int i = 1; i < 101; ++i) {
params.traj1[i][0] = traj1(i-1);
}
//cout << "hej" << endl;
params.lambda[0] = 10^12;
params.lambda[0] = 10000;
params.traj2_first[0] = traj2[0];
params.traj2_second[0] = traj2[1];
}
......
......@@ -6,7 +6,7 @@
/* Filename: testsolver.c. */
/* Description: Basic test harness for solver.c. */
#include "solver.h"
#include "cvxgen/solver.h"
#include <iostream>
#include <armadillo>
using namespace arma;
......
This diff is collapsed.
/* Produced by CVXGEN, 2016-01-17 08:15:56 -0500. */
/* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */
/* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */
/* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */
/* applications without prior written permission from Jacob Mattingley. */
/* Filename: solver.h. */
/* Description: Header file with relevant definitions. */
#ifndef SOLVER_H
#define SOLVER_H
/* Uncomment the next line to remove all library dependencies. */
/*#define ZERO_LIBRARY_MODE */
#ifdef MATLAB_MEX_FILE
/* Matlab functions. MATLAB_MEX_FILE will be defined by the mex compiler. */
/* If you are not using the mex compiler, this functionality will not intrude, */
/* as it will be completely disabled at compile-time. */
#include "mex.h"
#else
#ifndef ZERO_LIBRARY_MODE
#include <stdio.h>
#endif
#endif
/* Space must be allocated somewhere (testsolver.c, csolve.c or your own */
/* program) for the global variables vars, params, work and settings. */
/* At the bottom of this file, they are externed. */
#ifndef ZERO_LIBRARY_MODE
#include <math.h>
#define pm(A, m, n) printmatrix(#A, A, m, n, 1)
#endif
#include <armadillo>
using namespace arma;
typedef struct Params_t {
double traj1_1[1];
double traj1_2[1];
double traj1_3[1];
double traj1_4[1];
double traj1_5[1];
double traj1_6[1];
double traj1_7[1];
double traj1_8[1];
double traj1_9[1];
double traj1_10[1];
double traj1_11[1];
double traj1_12[1];
double traj1_13[1];
double traj1_14[1];
double traj1_15[1];
double traj1_16[1];
double traj1_17[1];
double traj1_18[1];
double traj1_19[1];
double traj1_20[1];
double traj1_21[1];
double traj1_22[1];
double traj1_23[1];
double traj1_24[1];
double traj1_25[1];
double traj1_26[1];
double traj1_27[1];
double traj1_28[1];
double traj1_29[1];
double traj1_30[1];
double traj1_31[1];
double traj1_32[1];
double traj1_33[1];
double traj1_34[1];
double traj1_35[1];
double traj1_36[1];
double traj1_37[1];
double traj1_38[1];
double traj1_39[1];
double traj1_40[1];
double traj1_41[1];
double traj1_42[1];
double traj1_43[1];
double traj1_44[1];
double traj1_45[1];
double traj1_46[1];
double traj1_47[1];
double traj1_48[1];
double traj1_49[1];
double traj1_50[1];
double traj1_51[1];
double traj1_52[1];
double traj1_53[1];
double traj1_54[1];
double traj1_55[1];
double traj1_56[1];
double traj1_57[1];
double traj1_58[1];
double traj1_59[1];
double traj1_60[1];
double traj1_61[1];
double traj1_62[1];
double traj1_63[1];
double traj1_64[1];
double traj1_65[1];
double traj1_66[1];
double traj1_67[1];
double traj1_68[1];
double traj1_69[1];
double traj1_70[1];
double traj1_71[1];
double traj1_72[1];
double traj1_73[1];
double traj1_74[1];
double traj1_75[1];
double traj1_76[1];
double traj1_77[1];
double traj1_78[1];
double traj1_79[1];
double traj1_80[1];
double traj1_81[1];
double traj1_82[1];
double traj1_83[1];
double traj1_84[1];
double traj1_85[1];
double traj1_86[1];
double traj1_87[1];
double traj1_88[1];
double traj1_89[1];
double traj1_90[1];
double traj1_91[1];
double traj1_92[1];
double traj1_93[1];
double traj1_94[1];
double traj1_95[1];
double traj1_96[1];
double traj1_97[1];
double traj1_98[1];
double traj1_99[1];
double traj1_100[1];
double lambda[1];
double traj2_first[1];
double traj2_second[1];
double *traj1[101];
} Params;
typedef struct Vars_t {
double *x_1; /* 1 rows. */
double *x_2; /* 1 rows. */
double *x_3; /* 1 rows. */
double *x_4; /* 1 rows. */
double *x_5; /* 1 rows. */
double *x_6; /* 1 rows. */
double *x_7; /* 1 rows. */
double *x_8; /* 1 rows. */
double *x_9; /* 1 rows. */
double *x_10; /* 1 rows. */
double *x_11; /* 1 rows. */
double *x_12; /* 1 rows. */
double *x_13; /* 1 rows. */
double *x_14; /* 1 rows. */
double *x_15; /* 1 rows. */
double *x_16; /* 1 rows. */
double *x_17; /* 1 rows. */
double *x_18; /* 1 rows. */
double *x_19; /* 1 rows. */
double *x_20; /* 1 rows. */
double *x_21; /* 1 rows. */
double *x_22; /* 1 rows. */
double *x_23; /* 1 rows. */
double *x_24; /* 1 rows. */
double *x_25; /* 1 rows. */
double *x_26; /* 1 rows. */
double *x_27; /* 1 rows. */
double *x_28; /* 1 rows. */
double *x_29; /* 1 rows. */
double *x_30; /* 1 rows. */
double *x_31; /* 1 rows. */
double *x_32; /* 1 rows. */
double *x_33; /* 1 rows. */
double *x_34; /* 1 rows. */
double *x_35; /* 1 rows. */
double *x_36; /* 1 rows. */
double *x_37; /* 1 rows. */
double *x_38; /* 1 rows. */
double *x_39; /* 1 rows. */
double *x_40; /* 1 rows. */
double *x_41; /* 1 rows. */
double *x_42; /* 1 rows. */
double *x_43; /* 1 rows. */
double *x_44; /* 1 rows. */
double *x_45; /* 1 rows. */
double *x_46; /* 1 rows. */
double *x_47; /* 1 rows. */
double *x_48; /* 1 rows. */
double *x_49; /* 1 rows. */
double *x_50; /* 1 rows. */
double *x_51; /* 1 rows. */
double *x_52; /* 1 rows. */
double *x_53; /* 1 rows. */
double *x_54; /* 1 rows. */
double *x_55; /* 1 rows. */
double *x_56; /* 1 rows. */
double *x_57; /* 1 rows. */
double *x_58; /* 1 rows. */
double *x_59; /* 1 rows. */
double *x_60; /* 1 rows. */
double *x_61; /* 1 rows. */
double *x_62; /* 1 rows. */
double *x_63; /* 1 rows. */
double *x_64; /* 1 rows. */
double *x_65; /* 1 rows. */
double *x_66; /* 1 rows. */
double *x_67; /* 1 rows. */
double *x_68; /* 1 rows. */
double *x_69; /* 1 rows. */
double *x_70; /* 1 rows. */
double *x_71; /* 1 rows. */
double *x_72; /* 1 rows. */
double *x_73; /* 1 rows. */
double *x_74; /* 1 rows. */
double *x_75; /* 1 rows. */
double *x_76; /* 1 rows. */
double *x_77; /* 1 rows. */
double *x_78; /* 1 rows. */
double *x_79; /* 1 rows. */
double *x_80; /* 1 rows. */
double *x_81; /* 1 rows. */
double *x_82; /* 1 rows. */
double *x_83; /* 1 rows. */
double *x_84; /* 1 rows. */
double *x_85; /* 1 rows. */
double *x_86; /* 1 rows. */
double *x_87; /* 1 rows. */
double *x_88; /* 1 rows. */
double *x_89; /* 1 rows. */
double *x_90; /* 1 rows. */
double *x_91; /* 1 rows. */
double *x_92; /* 1 rows. */
double *x_93; /* 1 rows. */
double *x_94;