first commit

Projects/SARAFun/sarafun_dmp/Makefile

+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

+#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

+#include <armadillo>
+using namespace arma;
+#ifndef ARMMOVING_H
+#define ARMMOVING_H
+
+bool armMoving(const mat& jointPos);
+
+#endif

Projects/SARAFun/sarafun_dmp/coupling.c

+#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

+/* 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

+#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

+#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

+# 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.m

+% 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