Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Martin Karlsson
sarafun_dmp
Commits
416bfa5d
Commit
416bfa5d
authored
Sep 22, 2016
by
Martin Karlsson
Browse files
first commit
parent
9a8ad662
Changes
62
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/armmoving.o
0 → 100644
View file @
416bfa5d
File added
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/coupling.o
0 → 100644
View file @
416bfa5d
File added
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.
Click to expand it.
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