Skip to content
GitLab
Commit 416bfa5d authored by Martin Karlsson's avatar Martin Karlsson
Browse files

first commit

parent 9a8ad662
Expand all Hide whitespace changes
Inline Side-by-side
Projects/SARAFun/sarafun_dmp/Makefile 0 → 100644
View file @ 416bfa5d
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
Projects/SARAFun/sarafun_dmp/armmoving.cc 0 → 100644
View file @ 416bfa5d
#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;
}
Projects/SARAFun/sarafun_dmp/armmoving.h 0 → 100644
View file @ 416bfa5d
#include <armadillo>
using namespace arma;
#ifndef ARMMOVING_H
#define ARMMOVING_H
bool armMoving(const mat& jointPos);
#endif
Projects/SARAFun/sarafun_dmp/coupling.c 0 → 100644
View file @ 416bfa5d
#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;
}
Projects/SARAFun/sarafun_dmp/coupling.h 0 → 100644
View file @ 416bfa5d
/* 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
Projects/SARAFun/sarafun_dmp/cuttraj.cc 0 → 100644
View file @ 416bfa5d
#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;
}
Projects/SARAFun/sarafun_dmp/cuttraj.h 0 → 100644
View file @ 416bfa5d
#include <armadillo>
using namespace arma;
#ifndef CUTTRAJ_H
#define CUTTRAJ_H
void cuttraj(const mat& evalTraj, const mat& corrTraj, mat& traj1, mat& traj2);
#endif
Projects/SARAFun/sarafun_dmp/cvxgen/Makefile 0 → 100644
View file @ 416bfa5d
# 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
Projects/SARAFun/sarafun_dmp/cvxgen/csolve.c 0 → 100644
View file @ 416bfa5d
This diff is collapsed.
Projects/SARAFun/sarafun_dmp/cvxgen/csolve.m 0 → 100644
View file @ 416bfa5d
% csolve Solves a custom quadratic program very rapidly.
%
% [vars, status] = csolve(params, settings)
%
% solves the convex optimization problem
%
% minimize(quad_form(traj1_1 - x_1, eye(1)) + quad_form(traj1_2 - x_2, eye(1)) + quad_form(traj1_3 - x_3, eye(1)) + quad_form(traj1_4 - x_4, eye(1)) + quad_form(traj1_5 - x_5, eye(1)) + quad_form(traj1_6 - x_6, eye(1)) + quad_form(traj1_7 - x_7, eye(1)) + quad_form(traj1_8 - x_8, eye(1)) + quad_form(traj1_9 - x_9, eye(1)) + quad_form(traj1_10 - x_10, eye(1)) + quad_form(traj1_11 - x_11, eye(1)) + quad_form(traj1_12 - x_12, eye(1)) + quad_form(traj1_13 - x_13, eye(1)) + quad_form(traj1_14 - x_14, eye(1)) + quad_form(traj1_15 - x_15, eye(1)) + quad_form(traj1_16 - x_16, eye(1)) + quad_form(traj1_17 - x_17, eye(1)) + quad_form(traj1_18 - x_18, eye(1)) + quad_form(traj1_19 - x_19, eye(1)) + quad_form(traj1_20 - x_20, eye(1)) + quad_form(traj1_21 - x_21, eye(1)) + quad_form(traj1_22 - x_22, eye(1)) + quad_form(traj1_23 - x_23, eye(1)) + quad_form(traj1_24 - x_24, eye(1)) + quad_form(traj1_25 - x_25, eye(1)) + quad_form(traj1_26 - x_26, eye(1)) + quad_form(traj1_27 - x_27, eye(1)) + quad_form(traj1_28 - x_28, eye(1)) + quad_form(traj1_29 - x_29, eye(1)) + quad_form(traj1_30 - x_30, eye(1)) + quad_form(traj1_31 - x_31, eye(1)) + quad_form(traj1_32 - x_32, eye(1)) + quad_form(traj1_33 - x_33, eye(1)) + quad_form(traj1_34 - x_34, eye(1)) + quad_form(traj1_35 - x_35, eye(1)) + quad_form(traj1_36 - x_36, eye(1)) + quad_form(traj1_37 - x_37, eye(1)) + quad_form(traj1_38 - x_38, eye(1)) + quad_form(traj1_39 - x_39, eye(1)) + quad_form(traj1_40 - x_40, eye(1)) + quad_form(traj1_41 - x_41, eye(1)) + quad_form(traj1_42 - x_42, eye(1)) + quad_form(traj1_43 - x_43, eye(1)) + quad_form(traj1_44 - x_44, eye(1)) + quad_form(traj1_45 - x_45, eye(1)) + quad_form(traj1_46 - x_46, eye(1)) + quad_form(traj1_47 - x_47, eye(1)) + quad_form(traj1_48 - x_48, eye(1)) + quad_form(traj1_49 - x_49, eye(1)) + quad_form(traj1_50 - x_50, eye(1)) + quad_form(traj1_51 - x_51, eye(1)) + quad_form(traj1_52 - x_52, eye(1)) + quad_form(traj1_53 - x_53, eye(1)) + quad_form(traj1_54 - x_54, eye(1)) + quad_form(traj1_55 - x_55, eye(1)) + quad_form(traj1_56 - x_56, eye(1)) + quad_form(traj1_57 - x_57, eye(1)) + quad_form(traj1_58 - x_58, eye(1)) + quad_form(traj1_59 - x_59, eye(1)) + quad_form(traj1_60 - x_60, eye(1)) + quad_form(traj1_61 - x_61, eye(1)) + quad_form(traj1_62 - x_62, eye(1)) + quad_form(traj1_63 - x_63, eye(1)) + quad_form(traj1_64 - x_64, eye(1)) + quad_form(traj1_65 - x_65, eye(1)) + quad_form(traj1_66 - x_66, eye(1)) + quad_form(traj1_67 - x_67, eye(1)) + quad_form(traj1_68 - x_68, eye(1)) + quad_form(traj1_69 - x_69, eye(1)) + quad_form(traj1_70 - x_70, eye(1)) + quad_form(traj1_71 - x_71, eye(1)) + quad_form(traj1_72 - x_72, eye(1)) + quad_form(traj1_73 - x_73, eye(1)) + quad_form(traj1_74 - x_74, eye(1)) + quad_form(traj1_75 - x_75, eye(1)) + quad_form(traj1_76 - x_76, eye(1)) + quad_form(traj1_77 - x_77, eye(1)) + quad_form(traj1_78 - x_78, eye(1)) + quad_form(traj1_79 - x_79, eye(1)) + quad_form(traj1_80 - x_80, eye(1)) + quad_form(traj1_81 - x_81, eye(1)) + quad_form(traj1_82 - x_82, eye(1)) + quad_form(traj1_83 - x_83, eye(1)) + quad_form(traj1_84 - x_84, eye(1)) + quad_form(traj1_85 - x_85, eye(1)) + quad_form(traj1_86 - x_86, eye(1)) + quad_form(traj1_87 - x_87, eye(1)) + quad_form(traj1_88 - x_88, eye(1)) + quad_form(traj1_89 - x_89, eye(1)) + quad_form(traj1_90 - x_90, eye(1)) + quad_form(traj1_91 - x_91, eye(1)) + quad_form(traj1_92 - x_92, eye(1)) + quad_form(traj1_93 - x_93, eye(1)) + quad_form(traj1_94 - x_94, eye(1)) + quad_form(traj1_95 - x_95, eye(1)) + quad_form(traj1_96 - x_96, eye(1)) + quad_form(traj1_97 - x_97, eye(1)) + quad_form(traj1_98 - x_98, eye(1)) + quad_form(traj1_99 - x_99, eye(1)) + quad_form(traj1_100 - x_100, eye(1)) + lambda*(quad_form(x_1 - 2*x_2 + x_3, eye(1)) + quad_form(x_2 - 2*x_3 + x_4, eye(1)) + quad_form(x_3 - 2*x_4 + x_5, eye(1)) + quad_form(x_4 - 2*x_5 + x_6, eye(1)) + quad_form(x_5 - 2*x_6 + x_7, eye(1)) + quad_form(x_6 - 2*x_7 + x_8, eye(1)) + quad_form(x_7 - 2*x_8 + x_9, eye(1)) + quad_form(x_8 - 2*x_9 + x_10, eye(1)) + quad_form(x_9 - 2*x_10 + x_11, eye(1)) + quad_form(x_10 - 2*x_11 + x_12, eye(1)) + quad_form(x_11 - 2*x_12 + x_13, eye(1)) + quad_form(x_12 - 2*x_13 + x_14, eye(1)) + quad_form(x_13 - 2*x_14 + x_15, eye(1)) + quad_form(x_14 - 2*x_15 + x_16, eye(1)) + quad_form(x_15 - 2*x_16 + x_17, eye(1)) + quad_form(x_16 - 2*x_17 + x_18, eye(1)) + quad_form(x_17 - 2*x_18 + x_19, eye(1)) + quad_form(x_18 - 2*x_19 + x_20, eye(1)) + quad_form(x_19 - 2*x_20 + x_21, eye(1)) + quad_form(x_20 - 2*x_21 + x_22, eye(1)) + quad_form(x_21 - 2*x_22 + x_23, eye(1)) + quad_form(x_22 - 2*x_23 + x_24, eye(1)) + quad_form(x_23 - 2*x_24 + x_25, eye(1)) + quad_form(x_24 - 2*x_25 + x_26, eye(1)) + quad_form(x_25 - 2*x_26 + x_27, eye(1)) + quad_form(x_26 - 2*x_27 + x_28, eye(1)) + quad_form(x_27 - 2*x_28 + x_29, eye(1)) + quad_form(x_28 - 2*x_29 + x_30, eye(1)) + quad_form(x_29 - 2*x_30 + x_31, eye(1)) + quad_form(x_30 - 2*x_31 + x_32, eye(1)) + quad_form(x_31 - 2*x_32 + x_33, eye(1)) + quad_form(x_32 - 2*x_33 + x_34, eye(1)) + quad_form(x_33 - 2*x_34 + x_35, eye(1)) + quad_form(x_34 - 2*x_35 + x_36, eye(1)) + quad_form(x_35 - 2*x_36 + x_37, eye(1)) + quad_form(x_36 - 2*x_37 + x_38, eye(1)) + quad_form(x_37 - 2*x_38 + x_39, eye(1)) + quad_form(x_38 - 2*x_39 + x_40, eye(1)) + quad_form(x_39 - 2*x_40 + x_41, eye(1)) + quad_form(x_40 - 2*x_41 + x_42, eye(1)) + quad_form(x_41 - 2*x_42 + x_43, eye(1)) + quad_form(x_42 - 2*x_43 + x_44, eye(1)) + quad_form(x_43 - 2*x_44 + x_45, eye(1)) + quad_form(x_44 - 2*x_45 + x_46, eye(1)) + quad_form(x_45 - 2*x_46 + x_47, eye(1)) + quad_form(x_46 - 2*x_47 + x_48, eye(1)) + quad_form(x_47 - 2*x_48 + x_49, eye(1)) + quad_form(x_48 - 2*x_49 + x_50, eye(1)) + quad_form(x_49 - 2*x_50 + x_51, eye(1)) + quad_form(x_50 - 2*x_51 + x_52, eye(1)) + quad_form(x_51 - 2*x_52 + x_53, eye(1)) + quad_form(x_52 - 2*x_53 + x_54, eye(1)) + quad_form(x_53 - 2*x_54 + x_55, eye(1)) + quad_form(x_54 - 2*x_55 + x_56, eye(1)) + quad_form(x_55 - 2*x_56 + x_57, eye(1)) + quad_form(x_56 - 2*x_57 + x_58, eye(1)) + quad_form(x_57 - 2*x_58 + x_59, eye(1)) + quad_form(x_58 - 2*x_59 + x_60, eye(1)) + quad_form(x_59 - 2*x_60 + x_61, eye(1)) + quad_form(x_60 - 2*x_61 + x_62, eye(1)) + quad_form(x_61 - 2*x_62 + x_63, eye(1)) + quad_form(x_62 - 2*x_63 + x_64, eye(1)) + quad_form(x_63 - 2*x_64 + x_65, eye(1)) + quad_form(x_64 - 2*x_65 + x_66, eye(1)) + quad_form(x_65 - 2*x_66 + x_67, eye(1)) + quad_form(x_66 - 2*x_67 + x_68, eye(1)) + quad_form(x_67 - 2*x_68 + x_69, eye(1)) + quad_form(x_68 - 2*x_69 + x_70, eye(1)) + quad_form(x_69 - 2*x_70 + x_71, eye(1)) + quad_form(x_70 - 2*x_71 + x_72, eye(1)) + quad_form(x_71 - 2*x_72 + x_73, eye(1)) + quad_form(x_72 - 2*x_73 + x_74, eye(1)) + quad_form(x_73 - 2*x_74 + x_75, eye(1)) + quad_form(x_74 - 2*x_75 + x_76, eye(1)) + quad_form(x_75 - 2*x_76 + x_77, eye(1)) + quad_form(x_76 - 2*x_77 + x_78, eye(1)) + quad_form(x_77 - 2*x_78 + x_79, eye(1)) + quad_form(x_78 - 2*x_79 + x_80, eye(1)) + quad_form(x_79 - 2*x_80 + x_81, eye(1)) + quad_form(x_80 - 2*x_81 + x_82, eye(1)) + quad_form(x_81 - 2*x_82 + x_83, eye(1)) + quad_form(x_82 - 2*x_83 + x_84, eye(1)) + quad_form(x_83 - 2*x_84 + x_85, eye(1)) + quad_form(x_84 - 2*x_85 + x_86, eye(1)) + quad_form(x_85 - 2*x_86 + x_87, eye(1)) + quad_form(x_86 - 2*x_87 + x_88, eye(1)) + quad_form(x_87 - 2*x_88 + x_89, eye(1)) + quad_form(x_88 - 2*x_89 + x_90, eye(1)) + quad_form(x_89 - 2*x_90 + x_91, eye(1)) + quad_form(x_90 - 2*x_91 + x_92, eye(1)) + quad_form(x_91 - 2*x_92 + x_93, eye(1)) + quad_form(x_92 - 2*x_93 + x_94, eye(1)) + quad_form(x_93 - 2*x_94 + x_95, eye(1)) + quad_form(x_94 - 2*x_95 + x_96, eye(1)) + quad_form(x_95 - 2*x_96 + x_97, eye(1)) + quad_form(x_96 - 2*x_97 + x_98, eye(1)) + quad_form(x_97 - 2*x_98 + x_99, eye(1)) + quad_form(x_98 - 2*x_99 + x_100, eye(1))))
% subject to
% x_100 == traj2_first
% x_99 - x_100 == traj2_first - traj2_second
%
% with variables
% x_1 1 x 1
% x_2 1 x 1
% x_3 1 x 1
% x_4 1 x 1
% x_5 1 x 1
% x_6 1 x 1
% x_7 1 x 1
% x_8 1 x 1
% x_9 1 x 1
% x_10 1 x 1
% x_11 1 x 1
% x_12 1 x 1
% x_13 1 x 1
% x_14 1 x 1
% x_15 1 x 1
% x_16 1 x 1
% x_17 1 x 1
% x_18 1 x 1
% x_19 1 x 1
% x_20 1 x 1
% x_21 1 x 1
% x_22 1 x 1
% x_23 1 x 1
% x_24 1 x 1
% x_25 1 x 1
% x_26 1 x 1
% x_27 1 x 1
% x_28 1 x 1
% x_29 1 x 1
% x_30 1 x 1
% x_31 1 x 1
% x_32 1 x 1
% x_33 1 x 1
% x_34 1 x 1
% x_35 1 x 1
% x_36 1 x 1
% x_37 1 x 1
% x_38 1 x 1
% x_39 1 x 1
% x_40 1 x 1
% x_41 1 x 1
% x_42 1 x 1
% x_43 1 x 1
% x_44 1 x 1
% x_45 1 x 1
% x_46 1 x 1
% x_47 1 x 1
% x_48 1 x 1
% x_49 1 x 1
% x_50 1 x 1
% x_51 1 x 1
% x_52 1 x 1
% x_53 1 x 1
% x_54 1 x 1
% x_55 1 x 1
% x_56 1 x 1
% x_57 1 x 1
% x_58 1 x 1
% x_59 1 x 1
% x_60 1 x 1
% x_61 1 x 1
% x_62 1 x 1