Commit 416bfa5d authored by Martin Karlsson's avatar Martin Karlsson
Browse files

first commit

parent 9a8ad662
CC=g++
CFLAGS=-I. -Wall -I/opt/robot/labcomm -I/opt/robot/orca \
-fpermissive -lpthread -std=c++11 -I/home/martinka/miscellaneous/armadillo-6.400.3/include -DARMA_DONT_USE_WRAPPER -llapack -lblas
LFLAGS=-lrt
LIBS=-lm -llapack -lblas
VPATH=/opt/robot/orca /opt/robot/labcomm
OBJSTATIC = orca_client.o orca_messages.o labcomm.o coupling.o traj2dmp.o \
dmp.o armmoving.o state2yardd.o record_ya.o twod_controller.o optpart.o \
solver.o cvxgen/matrix_support.o cvxgen/util.o cvxgen/ldl.o
all: main
%: %.cc
$(CC) -o $@ $(CFLAGS) $^ $(LIBS)
%.o: %.cc $(DEPS)
$(CC) $(CFLAGS) -c -o $@ $<
%.h %.c: %.lc
labcomm -C $<
main: $(OBJSTATIC) main.cc
$(CC) $(CFLAGS) $(LFLAGS) $(OBJSTATIC) main.cc -o main
static: $(OBJSTATIC)
ar rcs libcmt.a $^
clean:
rm -f main
rm -f *.o \
rm -f libcmt.a
rm -f libcmt.so
#include "armmoving.h"
#include <iostream>
#include <armadillo>
using namespace arma;
bool armMoving(const mat& jointPos) {
static bool ismoving = false;
double th = 0.002;
static int counter = 0;
static mat jointPosOld = zeros<mat>(1,7);
//static mat jointPosOldOld = zeros<mat>(1,7);
if (counter == 0) { // first run
jointPosOld = jointPos;
}
if (counter == 50) {
counter = 1;
if ( all(vectorise(sum(abs(jointPosOld - jointPos),1) > th))) {
ismoving = true;
} else {
ismoving = false;
}
jointPosOld = jointPos;
}
++counter;
return ismoving;
}
#include <armadillo>
using namespace arma;
#ifndef ARMMOVING_H
#define ARMMOVING_H
bool armMoving(const mat& jointPos);
#endif
#include "labcomm.h"
#include "labcomm_private.h"
#include "coupling.h"
static unsigned char signature_bytes_yaFromRobot[] = {
// array [14]
0, 0, 0, 16,
0, 0, 0, 1,
0, 0, 0, 14,
0, 0, 0, 37,
// }
};
labcomm_signature_t labcomm_signature_coupling_yaFromRobot = {
LABCOMM_SAMPLE, "yaFromRobot",
(int (*)(void *))labcomm_sizeof_coupling_yaFromRobot,
sizeof(signature_bytes_yaFromRobot),
signature_bytes_yaFromRobot
};
static unsigned char signature_bytes_yarddToRobot[] = {
// array [7]
0, 0, 0, 16,
0, 0, 0, 1,
0, 0, 0, 7,
0, 0, 0, 37,
// }
};
labcomm_signature_t labcomm_signature_coupling_yarddToRobot = {
LABCOMM_SAMPLE, "yarddToRobot",
(int (*)(void *))labcomm_sizeof_coupling_yarddToRobot,
sizeof(signature_bytes_yarddToRobot),
signature_bytes_yarddToRobot
};
static unsigned char signature_bytes_optsToRobot[] = {
// array [100]
0, 0, 0, 16,
0, 0, 0, 1,
0, 0, 0, 100,
0, 0, 0, 37,
// }
};
labcomm_signature_t labcomm_signature_coupling_optsToRobot = {
LABCOMM_SAMPLE, "optsToRobot",
(int (*)(void *))labcomm_sizeof_coupling_optsToRobot,
sizeof(signature_bytes_optsToRobot),
signature_bytes_optsToRobot
};
static void decode_yaFromRobot(
labcomm_decoder_t *d,
void (*handle)(
coupling_yaFromRobot *v,
void *context
),
void *context
)
{
coupling_yaFromRobot v;
{
int i_0_0;
for (i_0_0 = 0 ; i_0_0 < 14 ; i_0_0++) {
v.a[i_0_0] = labcomm_decode_float(d);
}
}
handle(&v, context);
}
void labcomm_decoder_register_coupling_yaFromRobot(
struct labcomm_decoder *d,
void (*handler)(
coupling_yaFromRobot *v,
void *context
),
void *context
)
{
labcomm_internal_decoder_register(
d,
&labcomm_signature_coupling_yaFromRobot,
(labcomm_decoder_typecast_t)decode_yaFromRobot,
(labcomm_handler_typecast_t)handler,
context
);
}
static void encode_yaFromRobot(
labcomm_encoder_t *e,
coupling_yaFromRobot *v
)
{
e->writer.write(&e->writer, labcomm_writer_start);
labcomm_encode_type_index(e, &labcomm_signature_coupling_yaFromRobot);
{
{
int i_0_0;
for (i_0_0 = 0 ; i_0_0 < 14 ; i_0_0++) {
labcomm_encode_float(e, (*v).a[i_0_0]);
}
}
}
e->writer.write(&e->writer, labcomm_writer_end);
}
void labcomm_encode_coupling_yaFromRobot(
labcomm_encoder_t *e,
coupling_yaFromRobot *v
)
{
labcomm_internal_encode(e, &labcomm_signature_coupling_yaFromRobot, v);
}
void labcomm_encoder_register_coupling_yaFromRobot(
struct labcomm_encoder *e
)
{
labcomm_internal_encoder_register(
e,
&labcomm_signature_coupling_yaFromRobot,
(labcomm_encode_typecast_t)encode_yaFromRobot
);
}
int labcomm_sizeof_coupling_yaFromRobot(coupling_yaFromRobot *v)
{
return 60;
}
static void decode_yarddToRobot(
labcomm_decoder_t *d,
void (*handle)(
coupling_yarddToRobot *v,
void *context
),
void *context
)
{
coupling_yarddToRobot v;
{
int i_0_0;
for (i_0_0 = 0 ; i_0_0 < 7 ; i_0_0++) {
v.a[i_0_0] = labcomm_decode_float(d);
}
}
handle(&v, context);
}
void labcomm_decoder_register_coupling_yarddToRobot(
struct labcomm_decoder *d,
void (*handler)(
coupling_yarddToRobot *v,
void *context
),
void *context
)
{
labcomm_internal_decoder_register(
d,
&labcomm_signature_coupling_yarddToRobot,
(labcomm_decoder_typecast_t)decode_yarddToRobot,
(labcomm_handler_typecast_t)handler,
context
);
}
static void encode_yarddToRobot(
labcomm_encoder_t *e,
coupling_yarddToRobot *v
)
{
e->writer.write(&e->writer, labcomm_writer_start);
labcomm_encode_type_index(e, &labcomm_signature_coupling_yarddToRobot);
{
{
int i_0_0;
for (i_0_0 = 0 ; i_0_0 < 7 ; i_0_0++) {
labcomm_encode_float(e, (*v).a[i_0_0]);
}
}
}
e->writer.write(&e->writer, labcomm_writer_end);
}
void labcomm_encode_coupling_yarddToRobot(
labcomm_encoder_t *e,
coupling_yarddToRobot *v
)
{
labcomm_internal_encode(e, &labcomm_signature_coupling_yarddToRobot, v);
}
void labcomm_encoder_register_coupling_yarddToRobot(
struct labcomm_encoder *e
)
{
labcomm_internal_encoder_register(
e,
&labcomm_signature_coupling_yarddToRobot,
(labcomm_encode_typecast_t)encode_yarddToRobot
);
}
int labcomm_sizeof_coupling_yarddToRobot(coupling_yarddToRobot *v)
{
return 32;
}
static void decode_optsToRobot(
labcomm_decoder_t *d,
void (*handle)(
coupling_optsToRobot *v,
void *context
),
void *context
)
{
coupling_optsToRobot v;
{
int i_0_0;
for (i_0_0 = 0 ; i_0_0 < 100 ; i_0_0++) {
v.a[i_0_0] = labcomm_decode_float(d);
}
}
handle(&v, context);
}
void labcomm_decoder_register_coupling_optsToRobot(
struct labcomm_decoder *d,
void (*handler)(
coupling_optsToRobot *v,
void *context
),
void *context
)
{
labcomm_internal_decoder_register(
d,
&labcomm_signature_coupling_optsToRobot,
(labcomm_decoder_typecast_t)decode_optsToRobot,
(labcomm_handler_typecast_t)handler,
context
);
}
static void encode_optsToRobot(
labcomm_encoder_t *e,
coupling_optsToRobot *v
)
{
e->writer.write(&e->writer, labcomm_writer_start);
labcomm_encode_type_index(e, &labcomm_signature_coupling_optsToRobot);
{
{
int i_0_0;
for (i_0_0 = 0 ; i_0_0 < 100 ; i_0_0++) {
labcomm_encode_float(e, (*v).a[i_0_0]);
}
}
}
e->writer.write(&e->writer, labcomm_writer_end);
}
void labcomm_encode_coupling_optsToRobot(
labcomm_encoder_t *e,
coupling_optsToRobot *v
)
{
labcomm_internal_encode(e, &labcomm_signature_coupling_optsToRobot, v);
}
void labcomm_encoder_register_coupling_optsToRobot(
struct labcomm_encoder *e
)
{
labcomm_internal_encoder_register(
e,
&labcomm_signature_coupling_optsToRobot,
(labcomm_encode_typecast_t)encode_optsToRobot
);
}
int labcomm_sizeof_coupling_optsToRobot(coupling_optsToRobot *v)
{
return 404;
}
/* LabComm declarations:
sample float yaFromRobot[14];
sample float yarddToRobot[7];
sample float optsToRobot[100];
*/
#ifndef __LABCOMM_coupling_H__
#define __LABCOMM_coupling_H__
#include "labcomm.h"
#ifndef PREDEFINED_coupling_yaFromRobot
typedef struct {
float a[14];
} coupling_yaFromRobot;
#endif
void labcomm_decoder_register_coupling_yaFromRobot(
struct labcomm_decoder *d,
void (*handler)(
coupling_yaFromRobot *v,
void *context
),
void *context
);
void labcomm_encoder_register_coupling_yaFromRobot(
struct labcomm_encoder *e);
void labcomm_encode_coupling_yaFromRobot(
struct labcomm_encoder *e,
coupling_yaFromRobot *v
);
extern int labcomm_sizeof_coupling_yaFromRobot(coupling_yaFromRobot *v);
#ifndef PREDEFINED_coupling_yarddToRobot
typedef struct {
float a[7];
} coupling_yarddToRobot;
#endif
void labcomm_decoder_register_coupling_yarddToRobot(
struct labcomm_decoder *d,
void (*handler)(
coupling_yarddToRobot *v,
void *context
),
void *context
);
void labcomm_encoder_register_coupling_yarddToRobot(
struct labcomm_encoder *e);
void labcomm_encode_coupling_yarddToRobot(
struct labcomm_encoder *e,
coupling_yarddToRobot *v
);
extern int labcomm_sizeof_coupling_yarddToRobot(coupling_yarddToRobot *v);
#ifndef PREDEFINED_coupling_optsToRobot
typedef struct {
float a[100];
} coupling_optsToRobot;
#endif
void labcomm_decoder_register_coupling_optsToRobot(
struct labcomm_decoder *d,
void (*handler)(
coupling_optsToRobot *v,
void *context
),
void *context
);
void labcomm_encoder_register_coupling_optsToRobot(
struct labcomm_encoder *e);
void labcomm_encode_coupling_optsToRobot(
struct labcomm_encoder *e,
coupling_optsToRobot *v
);
extern int labcomm_sizeof_coupling_optsToRobot(coupling_optsToRobot *v);
#define LABCOMM_FORALL_SAMPLES_coupling(func, sep) \
func(yaFromRobot, coupling_yaFromRobot) sep \
func(yarddToRobot, coupling_yarddToRobot) sep \
func(optsToRobot, coupling_optsToRobot)
#endif
#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);
//cout << dists << endl;
//cout << dist2old << endl;
cout << dist2oldmax << endl;
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;
}
}
cout << cutnew << endl;
// Cut old trajectory:
mat dist2cutnew = dists.col(cutnew);
//cout << dist2cutnew << endl;
double mindist2cutnew = as_scalar(min(dist2cutnew));
cout << mindist2cutnew << endl;
auto cutold = as_scalar(find(dist2cutnew == mindist2cutnew, 1, "first")) + 1;
cout << cutold << endl;
traj1 = evalTraj.rows(0,cutold-1);
traj2 = corrTraj.rows(cutnew-1, corrTraj.n_rows -1);
//cout << traj1 << endl;
//cout << traj2 << endl;
}
#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: Makefile.
# Description: Basic Makefile.
OPT = -Wall -Os
# libmath is needed for sqrt, which is used only for reporting the gap. Can
# remove if desired for production solvers..
LDLIBS = -lm
CFLAGS = $(OPT) $(INCLUDES)
CC = g++
.PHONY: all
all: testsolver
testsolver: solver.o matrix_support.o ldl.o testsolver.o util.o
# Include util.o for random functions and easy matrix printing.
#testsolver: solver.o matrix_support.o ldl.o util.o testsolver.o
solver.o: solver.h
matrix_support.o: solver.h
ldl.o: solver.h
util.o: solver.h
testsolver.o: solver.h
.PHONY : clean
clean :
-rm -f *.o testsolver
/* 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: csolve.c. */
/* Description: mex-able file for running cvxgen solver. */
#include "mex.h"
#include "solver.h"
Vars vars;
Params params;
Workspace work;
Settings settings;
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
int i, j;
mxArray *xm, *cell, *xm_cell;
double *src;
double *dest;
double *dest_cell;
int valid_vars;
int steps;
int this_var_errors;
int warned_diags;
int prepare_for_c = 0;
int extra_solves;
const char *status_names[] = {"optval", "gap", "steps", "converged"};
mwSize dims1x1of1[1] = {1};
mwSize dims[1];
const char *var_names[] = {"x_1", "x_2", "x_3", "x_4", "x_5", "x_6", "x_7", "x_8", "x_9", "x_10", "x_11", "x_12", "x_13", "x_14", "x_15", "x_16", "x_17", "x_18", "x_19", "x_20", "x_21", "x_22", "x_23", "x_24", "x_25", "x_26", "x_27", "x_28", "x_29", "x_30", "x_31", "x_32", "x_33", "x_34", "x_35", "x_36", "x_37", "x_38", "x_39", "x_40", "x_41", "x_42", "x_43", "x_44", "x_45", "x_46", "x_47", "x_48", "x_49", "x_50", "x_51", "x_52", "x_53", "x_54", "x_55", "x_56", "x_57", "x_58", "x_59", "x_60", "x_61", "x_62", "x_63", "x_64", "x_65", "x_66", "x_67", "x_68", "x_69", "x_70", "x_71", "x_72", "x_73", "x_74", "x_75", "x_76", "x_77", "x_78", "x_79", "x_80", "x_81", "x_82", "x_83", "x_84", "x_85", "x_86", "x_87", "x_88", "x_89", "x_90", "x_91", "x_92", "x_93", "x_94", "x_95", "x_96", "x_97", "x_98", "x_99", "x_100", "x"};
const int num_var_names = 101;
/* Avoid compiler warnings of unused variables by using a dummy assignment. */
warned_diags = j = 0;
extra_solves = 0;
set_defaults();
/* Check we got the right number of arguments. */
if (nrhs == 0)
mexErrMsgTxt("Not enough arguments: You need to specify at least the parameters.\n");
if (nrhs > 1) {
/* Assume that the second argument is the settings. */
if (mxGetField(prhs[1], 0, "eps") != NULL)
settings.eps = *mxGetPr(mxGetField(prhs[1], 0, "eps"));
if (mxGetField(prhs[1], 0, "max_iters") != NULL)
settings.max_iters = *mxGetPr(mxGetField(prhs[1], 0, "max_iters"));
if (mxGetField(prhs[1], 0, "refine_steps") != NULL)
settings.refine_steps = *mxGetPr(mxGetField(prhs[1], 0, "refine_steps"));
if (mxGetField(prhs[1], 0, "verbose") != NULL)
settings.verbose = *mxGetPr(mxGetField(prhs[1], 0, "verbose"));
if (mxGetField(prhs[1], 0, "better_start") != NULL)
settings.better_start = *mxGetPr(mxGetField(prhs[1], 0, "better_start"));
if (mxGetField(prhs[1], 0, "verbose_refinement") != NULL)
settings.verbose_refinement = *mxGetPr(mxGetField(prhs[1], 0,
"verbose_refinement"));
if (mxGetField(prhs[1], 0, "debug") != NULL)
settings.debug = *mxGetPr(mxGetField(prhs[1], 0, "debug"));
if (mxGetField(prhs[1], 0, "kkt_reg") != NULL)
settings.kkt_reg = *mxGetPr(mxGetField(prhs[1], 0, "kkt_reg"));
if (mxGetField(prhs[1], 0, "s_init") != NULL)
settings.s_init = *mxGetPr(mxGetField(prhs[1], 0, "s_init"));
if (mxGetField(prhs[1], 0, "z_init") != NULL)
settings.z_init = *mxGetPr(mxGetField(prhs[1], 0, "z_init"));
if (mxGetField(prhs[1], 0, "resid_tol") != NULL)
settings.resid_tol = *mxGetPr(mxGetField(prhs[1], 0, "resid_tol"));
if (mxGetField(prhs[1], 0, "extra_solves") != NULL)
extra_solves = *mxGetPr(mxGetField(prhs[1], 0, "extra_solves"));
else
extra_solves = 0;
if (mxGetField(prhs[1], 0, "prepare_for_c") != NULL)
prepare_for_c = *mxGetPr(mxGetField(prhs[1], 0, "prepare_for_c"));