Commit a8997807 authored by Martin Karlsson's avatar Martin Karlsson
Browse files

first commit

parents
#This is a somewhat generic .gitignore file template
# Compiled source #
###################
*.com
*.class
*.dll
*.exe
*.o
*.so
# Generated from texmaker#
############
*.pdf
*.aux
*.blg
*.dpth
*.md5
# Packages #
############
# it's better to unpack these files and commit the raw source
# git has its own built in compression methods
*.7z
*.dmg
*.gz
*.iso
*.jar
*.rar
*.tar
*.zip
# Logs and databases #
######################
*.log
*.sql
*.sqlite
CC=g++
CFLAGS=-I. -Wall \
-fpermissive -lpthread -std=c++11 -I~/.progs/armadillo-6.400.3/include -DARMA_DONT_USE_WRAPPER -larmadillo #-llapack -lblas
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
all: example_main
%: %.c
$(CC) -o $@ $(CFLAGS) $^ $(LIBS)
%.o: %.cpp $(DEPS)
$(CC) $(CFLAGS) -c -o $@ $<
example_main: $(OBJSTATIC) example_main.cc
$(CC) $(CFLAGS) $(LFLAGS) $(OBJSTATIC) example_main.cc -o example_main
static: $(OBJSTATIC)
ar rcs libcmt.a $^
clean:
rm -f example_main
rm -f *.o \
rm -f libcmt.a
rm -f libcmt.so
#include "cuttraj.h"
#include <iostream>
#include <armadillo>
using namespace arma;
void cuttraj(const mat& evalTraj, const mat& corrTraj, mat& traj1, mat& traj2) {
// Distances (2-norm):
mat dists = zeros<mat>(evalTraj.n_rows, corrTraj.n_rows);
for (int oldp = 0; oldp < evalTraj.n_rows; ++oldp) {
for (int newp = 0; newp < corrTraj.n_rows; ++newp) {
dists(oldp, newp) = norm( evalTraj.row(oldp) - corrTraj.row(newp));
}
}
mat dist2old = min(dists);
mat dist2oldmax = max(dist2old,1);
double th = 0.5;
int cutnew = 0;
bool haslonger = false;
for (int newp = dist2old.n_cols; newp > 0; --newp) {
if (dist2old(newp-1) > th*dist2oldmax(0,0)) {
cutnew = newp;
haslonger = 1;
} else if (haslonger) {
break;
}
}
// Cut old trajectory:
mat dist2cutnew = dists.col(cutnew);
double mindist2cutnew = as_scalar(min(dist2cutnew));
cout << mindist2cutnew << endl;
double cutold = as_scalar(find(dist2cutnew == mindist2cutnew, 1, "first")) + 1;
traj1 = evalTraj.rows(0,cutold-1);
traj2 = corrTraj.rows(cutnew-1, corrTraj.n_rows -1);
}
#include <armadillo>
using namespace arma;
#ifndef CUTTRAJ_H
#define CUTTRAJ_H
void cuttraj(const mat& evalTraj, const mat& corrTraj, mat& traj1, mat& traj2);
#endif
# Produced by CVXGEN, 2016-01-17 08:15:39 -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: description.cvxgen.
# Description: A description of the CVXGEN problem.
dimensions
N = 100
end
parameters
nbr_samples nonnegative
lambda nonnegative
traj1[i], i=1..N
traj2_first
traj2_second
end
variables
x[i], i=1..N
end
minimize
sum[i=1..N](quad(traj1[i] - x[i])) + lambda*sum[i=1..N-2](quad(x[i] - 2*x[i+1] + x[i+2]))
subject to
x[N] == traj2_first
x[N-1] - x[N] == traj2_first - traj2_second
end
This diff is collapsed.
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: util.c. */
/* Description: Common utility file for all cvxgen code. */
#include "solver.h"
#include <time.h>
#include <stdlib.h>
#include <math.h>
long global_seed = 1;
static clock_t tic_timestart;
void tic(void) {
tic_timestart = clock();
}
float toc(void) {
clock_t tic_timestop;
tic_timestop = clock();
printf("time: %8.2f.\n", (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC);
return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC;
}
float tocq(void) {
clock_t tic_timestop;
tic_timestop = clock();
return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC;
}
void printmatrix(char *name, double *A, int m, int n, int sparse) {
int i, j;
printf("%s = [...\n", name);
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++)
if ((sparse == 1) && (A[i+j*m] == 0))
printf(" 0");
else
printf(" % 9.4f", A[i+j*m]);
printf(",\n");
}
printf("];\n");
}
double unif(double lower, double upper) {
return lower + ((upper - lower)*rand())/RAND_MAX;
}
/* Next function is from numerical recipes in C. */
#define IA 16807
#define IM 2147483647
#define AM (1.0/IM)
#define IQ 127773
#define IR 2836
#define NTAB 32
#define NDIV (1+(IM-1)/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)
float ran1(long*idum, int reset) {
int j;
long k;
static long iy=0;
static long iv[NTAB];
float temp;
if (reset) {
iy = 0;
}
if (*idum<=0||!iy) {
if (-(*idum)<1)*idum=1;
else *idum=-(*idum);
for (j=NTAB+7; j>=0; j--) {
k = (*idum)/IQ;
*idum=IA*(*idum-k*IQ)-IR*k;
if (*idum<0)*idum+=IM;
if (j<NTAB)iv[j]=*idum;
}
iy = iv[0];
}
k = (*idum)/IQ;
*idum = IA*(*idum-k*IQ)-IR*k;
if (*idum<0)*idum += IM;
j = iy/NDIV;
iy = iv[j];
iv[j] = *idum;
if ((temp=AM*iy)> RNMX) return RNMX;
else return temp;
}
/* Next function is from numerical recipes in C. */
float randn_internal(long *idum, int reset) {
static int iset=0;
static float gset;
float fac, rsq, v1, v2;
if (reset) {
iset = 0;
}
if (iset==0) {
do {
v1 = 2.0*ran1(idum, reset)-1.0;
v2 = 2.0*ran1(idum, reset)-1.0;
rsq = v1*v1+v2*v2;
} while(rsq >= 1.0 || rsq == 0.0);
fac = sqrt(-2.0*log(rsq)/rsq);
gset = v1*fac;
iset = 1;
return v2*fac;
} else {
iset = 0;
return gset;
}
}
double randn(void) {
return randn_internal(&global_seed, 0);
}
void reset_rand(void) {
srand(15);
global_seed = 1;
randn_internal(&global_seed, 1);
}
#include "dmp.h"
#include <iostream>
#include <armadillo>
using namespace arma;
using namespace std;
Dmp::Dmp(mat w, mat g, double t) : weights(w), goal(g), tau(t) {}
mat Dmp::getW() const {
return weights;
}
mat Dmp::getG() const {
return goal;
}
double Dmp::getT() const {
return tau;
}
void Dmp::setG(mat g) {
goal = g;
}
void Dmp::setT(double t) {
tau = t;
}
Dmp& Dmp::doubleSpeed() {
tau = tau/2;
return *this;
}
Dmp& Dmp::speedupTimes(int x) {
tau = tau/x;
return *this;
}
ostream& operator<<(ostream& os, const Dmp& dmp) {
os << "Weights = " << dmp.weights << endl;
os << "Goal = " << dmp.goal << endl;
os << "Tau = " << dmp.tau << endl;
return os;
}
#include <armadillo>
using namespace arma;
using namespace std;
#ifndef DMP_H
#define DMP_H
class Dmp {
friend ostream& operator<<(ostream&, const Dmp&);
public:
Dmp(mat w, mat g, double t);
mat getW() const;
mat getG() const;
double getT() const;
void setG(mat g);
void setT(double t);
Dmp& doubleSpeed();
Dmp& speedupTimes(int x);
private:
mat weights;
mat goal;
double tau;
//new, to be integrated:
string ID;
string GroupID;
string log_url;
string name;
string comment;
string purpose;
string tool;
string robot;
int timestamp;
string controllertype; //e.g. extctrl
string robotcell;
string movementType;
string startTolerance;
string endTolerance;
string goalObject;
};
ostream& operator<<(ostream& os, const Dmp& dmp);
#endif
File added
#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;
}
#include "cuttraj.h"
#include <iostream>
#include <armadillo>
using namespace arma;
using namespace std;
mat 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;
mat file2mat(const string& filename, mat& y);
#endif
//simulink to fit_dmp
sample float trajectoryFromRobot[8];
//fit_dmp to simulink
sample float wToRobot[105];
sample float GToRobot[7];
sample float tauToRobot;
/* 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. */
/* Description: Basic test harness for solver.c. */
#include "solver.h"
#include "optpart.h"
#include <iostream>
#include <armadillo>
using namespace arma;
using namespace std;
Vars vars;
Params params;
Workspace work;
Settings settings;
mat optpart(const mat& traj1, const mat& traj2) {
set_defaults();
setup_indexing();
load_default_data(traj1, traj2);
settings.verbose = 1;
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;
}
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.traj2_first[0] = traj2[0];
params.traj2_second[0] = traj2[1];
}
/* 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: testsolver.c. */
/* Description: Basic test harness for solver.c. */
#include "solver.h"
#include <iostream>
#include <armadillo>
using namespace arma;
using namespace std;
mat optpart(const mat& traj1, const mat& traj2);
void load_default_data(const mat& traj1, const mat& traj2);
File added
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];