Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
ndcurves
Commits
7a8e77f9
Commit
7a8e77f9
authored
Jan 09, 2020
by
stevet
Committed by
Pierre Fernbach
May 04, 2020
Browse files
remove cubic and quintic classes
parent
2f385541
Changes
5
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
7a8e77f9
...
...
@@ -58,9 +58,7 @@ SET(${PROJECT_NAME}_HEADERS
include/
${
PROJECT_NAME
}
/MathDefs.h
include/
${
PROJECT_NAME
}
/polynomial.h
include/
${
PROJECT_NAME
}
/bezier_curve.h
include/
${
PROJECT_NAME
}
/cubic_spline.h
include/
${
PROJECT_NAME
}
/curve_constraint.h
include/
${
PROJECT_NAME
}
/quintic_spline.h
include/
${
PROJECT_NAME
}
/linear_variable.h
include/
${
PROJECT_NAME
}
/quadratic_variable.h
include/
${
PROJECT_NAME
}
/cubic_hermite_spline.h
...
...
include/curves/cubic_spline.h
deleted
100644 → 0
View file @
2f385541
/**
* \file cubic_spline.h
* \brief Definition of a cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the CubicFunction struct.
* It allows the creation and evaluation of natural
* smooth cubic splines of arbitrary dimension
*/
#ifndef _STRUCT_CUBICSPLINE
#define _STRUCT_CUBICSPLINE
#include
"MathDefs.h"
#include
"polynomial.h"
#include
<stdexcept>
namespace
curves
{
/// \brief Creates coefficient vector of a cubic spline defined on the interval
/// \f$[t_{min}, t_{max}]\f$. It follows the equation : <br>
/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$
/// with a, b, c and d the control points.
///
template
<
typename
Point
,
typename
T_Point
>
T_Point
make_cubic_vector
(
Point
const
&
a
,
Point
const
&
b
,
Point
const
&
c
,
Point
const
&
d
)
{
T_Point
res
;
res
.
push_back
(
a
);
res
.
push_back
(
b
);
res
.
push_back
(
c
);
res
.
push_back
(
d
);
return
res
;
}
template
<
typename
Time
,
typename
Numeric
,
bool
Safe
,
typename
Point
,
typename
T_Point
>
polynomial
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
create_cubic
(
Point
const
&
a
,
Point
const
&
b
,
Point
const
&
c
,
Point
const
&
d
,
const
Time
t_min
,
const
Time
t_max
)
{
T_Point
coeffs
=
make_cubic_vector
<
Point
,
T_Point
>
(
a
,
b
,
c
,
d
);
return
polynomial
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
(
coeffs
.
begin
(),
coeffs
.
end
(),
t_min
,
t_max
);
}
}
// namespace curves
#endif //_STRUCT_CUBICSPLINE
include/curves/exact_cubic.h
View file @
7a8e77f9
...
...
@@ -20,10 +20,9 @@
#define _CLASS_EXACTCUBIC
#include
"curve_abc.h"
#include
"cubic_spline.h"
#include
"quintic_spline.h"
#include
"curve_constraint.h"
#include
"piecewise_curve.h"
#include
"polynomial.h"
#include
"MathDefs.h"
...
...
@@ -41,6 +40,7 @@ template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename
SplineBase
=
polynomial
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
>
struct
exact_cubic
:
public
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
const
Eigen
::
Ref
<
const
point_t
>
point_ref_t
;
typedef
T_Point
t_point_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Eigen
::
Matrix
<
Numeric
,
3
,
3
>
Matrix3
;
...
...
@@ -52,9 +52,10 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
typedef
typename
t_spline_t
::
const_iterator
cit_spline_t
;
typedef
curve_constraints
<
Point
>
spline_constraints
;
typedef
exact_cubic
<
Time
,
Numeric
,
Safe
,
P
oint
,
T_Point
,
SplineBase
>
exact_cubic_t
;
typedef
exact_cubic
<
Time
,
Numeric
,
Safe
,
p
oint
_t
,
T_Point
,
SplineBase
>
exact_cubic_t
;
typedef
curve_abc
<
Time
,
Numeric
,
Safe
,
point_t
>
curve_abc_t
;
// parent class
typedef
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
>
piecewise_curve_t
;
typedef
piecewise_curve
<
Time
,
Numeric
,
Safe
,
point_t
>
piecewise_curve_t
;
typedef
polynomial
<
Time
,
Numeric
,
Safe
,
point_t
>
polynomial_t
;
typedef
typename
piecewise_curve_t
::
t_curve_ptr_t
t_curve_ptr_t
;
/* Constructors - destructors */
...
...
@@ -128,6 +129,29 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
}
private:
static
polynomial_t
create_cubic
(
point_ref_t
a
,
point_ref_t
b
,
point_ref_t
c
,
point_ref_t
d
,
const
time_t
t_min
,
const
time_t
t_max
){
typename
polynomial_t
::
t_point_t
coeffs
;
coeffs
.
push_back
(
a
);
coeffs
.
push_back
(
b
);
coeffs
.
push_back
(
c
);
coeffs
.
push_back
(
d
);
return
polynomial_t
(
coeffs
.
begin
(),
coeffs
.
end
(),
t_min
,
t_max
);
}
static
polynomial_t
create_quintic
(
point_ref_t
a
,
point_ref_t
b
,
point_ref_t
c
,
point_ref_t
d
,
point_ref_t
e
,
point_ref_t
f
,
const
time_t
t_min
,
const
time_t
t_max
){
typename
polynomial_t
::
t_point_t
coeffs
;
coeffs
.
push_back
(
a
);
coeffs
.
push_back
(
b
);
coeffs
.
push_back
(
c
);
coeffs
.
push_back
(
d
);
coeffs
.
push_back
(
e
);
coeffs
.
push_back
(
f
);
return
polynomial_t
(
coeffs
.
begin
(),
coeffs
.
end
(),
t_min
,
t_max
);
}
/// \brief Compute polynom of exact cubic spline from waypoints.
/// Compute the coefficients of polynom as in paper : "Task-Space Trajectories via Cubic Spline Optimization".<br>
/// \f$x_i(t)=a_i+b_i(t-t_i)+c_i(t-t_i)^2\f$<br>
...
...
@@ -198,7 +222,7 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
it
=
wayPointsBegin
,
next
=
wayPointsBegin
;
++
next
;
for
(
int
i
=
0
;
next
!=
wayPointsEnd
;
++
i
,
++
it
,
++
next
)
{
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),
subSplines
.
push_back
(
create_cubic
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),
(
*
it
).
first
,
(
*
next
).
first
));
}
return
subSplines
;
...
...
@@ -230,7 +254,7 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
const
num_t
&
init_t
=
wayPointsBegin
->
first
,
end_t
=
wayPointsNext
->
first
;
const
num_t
dt
=
end_t
-
init_t
,
dt_2
=
dt
*
dt
,
dt_3
=
dt_2
*
dt
;
const
point_t
d0
=
(
a1
-
a0
-
b0
*
dt
-
c0
*
dt_2
)
/
dt_3
;
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
(
a0
,
b0
,
c0
,
d0
,
init_t
,
end_t
));
subSplines
.
push_back
(
create_cubic
(
a0
,
b0
,
c0
,
d0
,
init_t
,
end_t
));
constraints
.
init_vel
=
subSplines
.
back
().
derivate
(
end_t
,
1
);
constraints
.
init_acc
=
subSplines
.
back
().
derivate
(
end_t
,
2
);
}
...
...
@@ -270,7 +294,7 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
d
=
rhs
.
row
(
0
);
e
=
rhs
.
row
(
1
);
f
=
rhs
.
row
(
2
);
subSplines
.
push_back
(
create_quintic
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
(
a0
,
b0
,
c0
,
d
,
e
,
f
,
init_t
,
end_t
));
subSplines
.
push_back
(
create_quintic
(
a0
,
b0
,
c0
,
d
,
e
,
f
,
init_t
,
end_t
));
}
public:
...
...
include/curves/optimization/OptimizeSpline.h
deleted
100644 → 0
View file @
2f385541
/**
* \file OptimizeSpline.h
* \brief Optimization loop for cubic spline generations
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file uses the mosek library to optimize the waypoints location
* to generate exactCubic spline
*/
#ifndef _CLASS_SPLINEOPTIMIZER
#define _CLASS_SPLINEOPTIMIZER
#include
"curves/MathDefs.h"
#include
"curves/exact_cubic.h"
#include
"mosek/mosek.h"
#include
<Eigen/SparseCore>
#include
<utility>
namespace
curves
{
/// \class SplineOptimizer
/// \brief Mosek connection to produce optimized splines
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
SplineOptimizer
{
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
exact_cubic
<
time_t
,
Numeric
,
Dim
,
Safe
,
Point
>
exact_cubic_t
;
typedef
SplineOptimizer
<
time_t
,
Numeric
,
Dim
,
Safe
,
Point
>
splineOptimizer_t
;
/* Constructors - destructors */
public:
///\brief Initializes optimizer environment.
SplineOptimizer
()
{
MSKrescodee
r_
=
MSK_makeenv
(
&
env_
,
NULL
);
if
(
r_
!=
MSK_RES_OK
)
{
throw
std
::
runtime_error
(
"Issue initializing MSK_makeenv"
);
}
}
///\brief Destructor.
~
SplineOptimizer
()
{
MSK_deleteenv
(
&
env_
);
}
private:
SplineOptimizer
(
const
SplineOptimizer
&
);
SplineOptimizer
&
operator
=
(
const
SplineOptimizer
&
);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Start an optimization loop to create curve.
/// \param waypoints : a list containing at least 2 waypoints in ascending time order.
/// \return An Optimised curve.
template
<
typename
In
>
exact_cubic_t
*
GenerateOptimizedCurve
(
In
wayPointsBegin
,
In
wayPointsEnd
)
const
;
/*Operations*/
private:
template
<
typename
In
>
void
ComputeHMatrices
(
In
wayPointsBegin
,
In
wayPointsEnd
,
MatrixX
&
h1
,
MatrixX
&
h2
,
MatrixX
&
h3
,
MatrixX
&
h4
)
const
;
/*Attributes*/
MSKenv_t
env_
;
/*Attributes*/
private:
typedef
std
::
pair
<
time_t
,
Point
>
waypoint_t
;
typedef
std
::
vector
<
waypoint_t
>
T_waypoints_t
;
};
// End struct SplineOptimizer
template
<
typename
Time
,
typename
Numeric
,
std
::
size_t
Dim
,
bool
Safe
,
typename
Point
>
template
<
typename
In
>
inline
void
SplineOptimizer
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>::
ComputeHMatrices
(
In
wayPointsBegin
,
In
wayPointsEnd
,
MatrixX
&
h1
,
MatrixX
&
h2
,
MatrixX
&
h3
,
MatrixX
&
h4
)
const
{
std
::
size_t
const
size
(
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
));
assert
((
!
Safe
)
||
(
size
>
1
));
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
);
++
next
;
Numeric
t_previous
((
*
it
).
first
);
for
(
std
::
size_t
i
(
0
);
next
!=
wayPointsEnd
;
++
next
,
++
it
,
++
i
)
{
num_t
const
dTi
((
*
next
).
first
-
(
*
it
).
first
);
num_t
const
dTi_sqr
(
dTi
*
dTi
);
// filling matrices values
h3
(
i
,
i
)
=
-
3
/
dTi_sqr
;
h3
(
i
,
i
+
1
)
=
3
/
dTi_sqr
;
h4
(
i
,
i
)
=
-
2
/
dTi
;
h4
(
i
,
i
+
1
)
=
-
1
/
dTi
;
if
(
i
+
2
<
size
)
{
In
it2
(
next
);
++
it2
;
num_t
const
dTi_1
((
*
it2
).
first
-
(
*
next
).
first
);
num_t
const
dTi_1sqr
(
dTi_1
*
dTi_1
);
// this can be optimized but let's focus on clarity as long as not needed
h1
(
i
+
1
,
i
)
=
2
/
dTi
;
h1
(
i
+
1
,
i
+
1
)
=
4
/
dTi
+
4
/
dTi_1
;
h1
(
i
+
1
,
i
+
2
)
=
2
/
dTi_1
;
h2
(
i
+
1
,
i
)
=
-
6
/
dTi_sqr
;
h2
(
i
+
1
,
i
+
1
)
=
(
6
/
dTi_1sqr
)
-
(
6
/
dTi_sqr
);
h2
(
i
+
1
,
i
+
2
)
=
6
/
dTi_1sqr
;
}
}
}
template
<
typename
Time
,
typename
Numeric
,
std
::
size_t
Dim
,
bool
Safe
,
typename
Point
>
template
<
typename
In
>
inline
exact_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>*
SplineOptimizer
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>::
GenerateOptimizedCurve
(
In
wayPointsBegin
,
In
wayPointsEnd
)
const
{
exact_cubic_t
*
res
=
0
;
int
const
size
((
int
)
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
));
if
(
Safe
&&
size
<
1
)
{
throw
std
::
length_error
(
"can not generate optimizedCurve, number of waypoints should be superior to one"
);
;
// TODO
}
// refer to the paper to understand all this.
MatrixX
h1
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h2
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h3
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h4
=
MatrixX
::
Zero
(
size
,
size
);
// remove this for the time being
/*MatrixX g1 = MatrixX::Zero(size, size);
MatrixX g2 = MatrixX::Zero(size, size);
MatrixX g3 = MatrixX::Zero(size, size);
MatrixX g4 = MatrixX::Zero(size, size);*/
ComputeHMatrices
(
wayPointsBegin
,
wayPointsEnd
,
h1
,
h2
,
h3
,
h4
);
// number of Waypoints : T + 1 => T mid points. Dim variables per points, + acceleration + derivations
// (T * t+ 1 ) * Dim * 3 = nb var
// NOG const MSKint32t numvar = ( size + size - 1) * 3 * 3;
const
MSKint32t
numvar
=
(
size
)
*
Dim
*
3
;
/*
We store the variables in that order to simplifly matrix computation ( see later )
// NOG [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn.. x0' --- zn..' ] T
[ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn..] T
*/
/*the first constraint is H1x. = H2x => H2x - H1x. = 0
this will give us size * 3 inequalities constraints
So this line of A will be writen
H2 -H1 0 0 0 0
*/
int
ptOff
=
(
int
)
Dim
*
size
;
// . offest
int
ptptOff
=
(
int
)
Dim
*
2
*
size
;
// .. offest
int
prOff
=
(
int
)
3
*
ptOff
;
// ' offest
// NOG int prptOff = (int) prOff + ptOff; // '. offest
// NOG int prptptOff = (int) prptOff + ptOff; // '.. offest
MatrixX
h2x_h1x
=
MatrixX
::
Zero
(
size
*
Dim
,
numvar
);
/**A looks something like that : (n = size)
[H2(0) 0 0 -H1(0) 0-------------------0]
[ 0 0 H2(0) 0 0 -H1(0)---------------0]
[ 0 0 0 H2(0) 0 0 H1(0)--------0]
...
[ 0 0 0 0 H2(n) 0 0 0 -H1(n)-0] // row n
Overall it's fairly easy to fill
*/
for
(
int
i
=
0
;
i
<
size
*
Dim
;
i
=
i
+
3
)
{
for
(
int
j
=
0
;
j
<
Dim
;
++
j
)
{
int
id
=
i
+
j
;
h2x_h1x
.
block
(
id
,
j
*
size
,
1
,
size
)
=
h2
.
row
(
i
%
3
);
h2x_h1x
.
block
(
id
,
j
*
size
+
ptOff
,
1
,
size
)
-=
h1
.
row
(
i
%
3
);
}
}
/*the second constraint is x' = G1x + G2x. => G1x + G2x. - x' = 0
this will give us size * 3 inequalities constraints
So this line of A will be writen
H2 -H1 0 0 0 0
*/
MatrixX
g1x_g2x
=
MatrixX
::
Zero
(
size
*
Dim
,
numvar
);
/**A looks something like that : (n = size)
[G1(0) 0 0 G2(0) 0-----------------------0 -1 0]
[ 0 0 G1(0) 0 0 G2(0)-------------------0 -1 0]
[ 0 0 0 G1(0) 0 0 G2(0)-----------0 -1 0]
...
[ 0 0 0 0 G1(n) 0 0 0 G2(n)-0 -1 0] // row n
Overall it's fairly easy to fill
*/
// NOG
/*for(int j = 0; j<3; ++j)
{
for(int j =0; j<3; ++j)
{
int id = i + j;
g1x_g2x.block(id, j*size , 1, size) = g1.row(i);
g1x_g2x.block(id, j*size + ptOff, 1, size) = g2.row(i);
g1x_g2x.block(id, j*size + prOff, 1, size) -= MatrixX::Ones(1, size);
}
}*/
/*the third constraint is x.' = G3x + G4x. => G3x + G4x. - x.' = 0
this will give us size * 3 inequalities constraints
So this line of A will be writen
H2 -H1 0 0 0 0
*/
MatrixX
g3x_g4x
=
MatrixX
::
Zero
(
size
*
Dim
,
numvar
);
/**A looks something like that : (n = size)
[G3(0) 0 0 G4(0) 0-------------------0 -1 0]
[ 0 0 G3(0) 0 0 G4(0)---------------0 -1 0]
[ 0 0 0 G3(0) 0 0 G4(0)--------0 -1 0]
...
[ 0 0 0 0 G3(n) 0 0 0 G4(n)-0 -1 0] // row n
Overall it's fairly easy to fill
*/
// NOG
/*for(int j = 0; j<3; ++j)
{
for(int j =0; j<3; ++j)
{
int id = i + j;
g3x_g4x.block(id, j*size , 1, size) = g3.row(i);
g3x_g4x.block(id, j*size + ptOff, 1, size) = g4.row(i);
g3x_g4x.block(id, j*size + prptOff, 1, size) -= MatrixX::Ones(1, size);
}
}
*/
/*the fourth constraint is x.. = 1/2(H3x + H4x.) => 1/2(H3x + H4x.) - x.. = 0
=> H3x + H4x. - 2x.. = 0
this will give us size * 3 inequalities constraints
So this line of A will be writen
H2 -H1 0 0 0 0
*/
MatrixX
h3x_h4x
=
MatrixX
::
Zero
(
size
*
Dim
,
numvar
);
/**A looks something like that : (n = size)
[H3(0) 0 0 H4(0) 0-------------------0 -2 0]
[ 0 0 H3(0) 0 0 H4(0)---------------0 -2 0]
[ 0 0 0 H3(0) 0 0 H4(0)-------0 -2 0]
...
[ 0 0 0 0 H3(n) 0 0 0 H4(n)-0 -2 0] // row n
Overall it's fairly easy to fill
*/
for
(
int
i
=
0
;
i
<
size
*
Dim
;
i
=
i
+
Dim
)
{
for
(
int
j
=
0
;
j
<
Dim
;
++
j
)
{
int
id
=
i
+
j
;
h3x_h4x
.
block
(
id
,
j
*
size
,
1
,
size
)
=
h3
.
row
(
i
%
3
);
h3x_h4x
.
block
(
id
,
j
*
size
+
ptOff
,
1
,
size
)
=
h4
.
row
(
i
%
3
);
h3x_h4x
.
block
(
id
,
j
*
size
+
ptptOff
,
1
,
size
)
=
MatrixX
::
Ones
(
1
,
size
)
*
-
2
;
}
}
/*the following constraints are easy to understand*/
/*x0,: = x^0*/
MatrixX
x0_x0
=
MatrixX
::
Zero
(
Dim
,
numvar
);
for
(
int
j
=
0
;
j
<
Dim
;
++
j
)
{
x0_x0
(
0
,
0
)
=
1
;
x0_x0
(
1
,
size
)
=
1
;
x0_x0
(
2
,
size
*
2
)
=
1
;
}
/*x0.,: = 0*/
MatrixX
x0p_0
=
MatrixX
::
Zero
(
Dim
,
numvar
);
for
(
int
j
=
0
;
j
<
Dim
;
++
j
)
{
x0p_0
(
0
,
ptOff
)
=
1
;
x0p_0
(
1
,
ptOff
+
size
)
=
1
;
x0p_0
(
2
,
ptOff
+
size
*
2
)
=
1
;
}
/*xt,: = x^t*/
MatrixX
xt_xt
=
MatrixX
::
Zero
(
Dim
,
numvar
);
for
(
int
j
=
0
;
j
<
Dim
;
++
j
)
{
xt_xt
(
0
,
size
-
1
)
=
1
;
xt_xt
(
1
,
2
*
size
-
1
)
=
1
;
xt_xt
(
2
,
3
*
size
-
1
)
=
1
;
}
/*xT.,: = 0*/
MatrixX
xtp_0
=
MatrixX
::
Zero
(
Dim
,
numvar
);
for
(
int
j
=
0
;
j
<
Dim
;
++
j
)
{
xtp_0
(
0
,
ptOff
+
size
-
1
)
=
1
;
xtp_0
(
1
,
ptOff
+
size
+
size
-
1
)
=
1
;
xtp_0
(
2
,
ptOff
+
size
*
2
+
size
-
1
)
=
1
;
}
// skipping constraints on x and y accelerations for the time being
// to compute A i'll create an eigen matrix, then i'll convert it to a sparse one and fill those tables
// total number of constraints
// NOG h2x_h1x (size * Dim) + h3x_h4x (size * Dim ) + g1x_g2x (size * Dim ) + g3x_g4x (size*Dim)
// NOG + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 4 * Dim * size + 4 * Dim
// h2x_h1x (size * Dim) + h3x_h4x (size * Dim )
// + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 2 * Dim * size + 4 * Dim
// NOG const MSKint32t numcon = 12 * size + 12;
const
MSKint32t
numcon
=
Dim
*
2
*
size
+
4
*
Dim
;
// TODO
// this gives us the matrix A of size numcon * numvaar
MatrixX
a
=
MatrixX
::
Zero
(
numcon
,
numvar
);
a
.
block
(
0
,
0
,
size
*
Dim
,
numvar
)
=
h2x_h1x
;
a
.
block
(
size
*
Dim
,
0
,
size
*
Dim
,
numvar
)
=
h3x_h4x
;
a
.
block
(
size
*
Dim
*
2
,
0
,
Dim
,
numvar
)
=
x0p_0
;
a
.
block
(
size
*
Dim
*
2
+
Dim
,
0
,
Dim
,
numvar
)
=
xtp_0
;
a
.
block
(
size
*
Dim
*
2
+
Dim
*
2
,
0
,
Dim
,
numvar
)
=
x0_x0
;
a
.
block
(
size
*
Dim
*
2
+
Dim
*
3
,
0
,
Dim
,
numvar
)
=
xt_xt
;
// convert to sparse representation
Eigen
::
SparseMatrix
<
Numeric
>
spA
;
spA
=
a
.
sparseView
();
// convert to sparse representation using column
// http://docs.mosek.com/7.0/capi/Conventions_employed_in_the_API.html#sec-intro-subsubsec-cmo-rmo-matrix
int
nonZeros
=
spA
.
nonZeros
();
/* Below is the sparse representation of the A
matrix stored by column. */
double
*
aval
=
new
double
[
nonZeros
];
MSKint32t
*
asub
=
new
MSKint32t
[
nonZeros
];
MSKint32t
*
aptrb
=
new
MSKint32t
[
numvar
];
MSKint32t
*
aptre
=
new
MSKint32t
[
numvar
];
int
currentIndex
=
0
;
for
(
int
j
=
0
;
j
<
numvar
;
++
j
)
{
bool
nonZeroAtThisCol
=
false
;
for
(
int
i
=
0
;
i
<
numcon
;
++
i
)
{
if
(
a
(
i
,
j
)
!=
0
)
{
if
(
!
nonZeroAtThisCol
)
{
aptrb
[
j
]
=
currentIndex
;
nonZeroAtThisCol
=
true
;
}
aval
[
currentIndex
]
=
a
(
i
,
j
);
asub
[
currentIndex
]
=
i
;
aptre
[
j
]
=
currentIndex
+
1
;
// overriding previous value
++
currentIndex
;
}
}
}
/*Q looks like this
0 0 0 0 0 0 | -> size * 3
0 0 2 0 0 0 | -> size *3
0 0 0 0 0 0
0 0 0 0 2 0
0 0 0 0 0 0
*/
/* Number of non-zeros in Q.*/
const
MSKint32t
numqz
=
size
*
Dim
*
2
;
MSKint32t
*
qsubi
=
new
MSKint32t
[
numqz
];
MSKint32t
*
qsubj
=
new
MSKint32t
[
numqz
];
double
*
qval
=
new
double
[
numqz
];
for
(
int
id
=
0
;
id
<
numqz
;
++
id
)
{
qsubi
[
id
]
=
id
+
ptOff
;
// we want the x.
qsubj
[
id
]
=
id
+
ptOff
;
qval
[
id
]
=
2
;
}
/* Bounds on constraints. */
MSKboundkeye
*
bkc
=
new
MSKboundkeye
[
numcon
];
double
*
blc
=
new
double
[
numcon
];
double
*
buc
=
new
double
[
numcon
];
for
(
int
i
=
0
;
i
<
numcon
-
Dim
*
2
;
++
i
)
{
bkc
[
i
]
=
MSK_BK_FX
;
blc
[
i
]
=
0
;
buc
[
i
]
=
0
;
}
for
(
int
i
=
numcon
-
Dim
*
2
;
i
<
numcon
-
Dim
;
++
i
)
// x0 = x^0
{
bkc
[
i
]
=
MSK_BK_FX
;
blc
[
i
]
=
wayPointsBegin
->
second
(
i
-
(
numcon
-
Dim
*
2
));
buc
[
i
]
=
wayPointsBegin
->
second
(
i
-
(
numcon
-
Dim
*
2
));
}
In
last
(
wayPointsEnd
);
--
last
;
for
(
int
i
=
numcon
-
3
;
i
<
numcon
;
++
i
)
// xT = x^T
{
bkc
[
i
]
=
MSK_BK_FX
;
blc
[
i
]
=
last
->
second
(
i
-
(
numcon
-
Dim
));
buc
[
i
]
=
last
->
second
(
i
-
(
numcon
-
Dim
));
}
///*No Bounds on variables. */
MSKboundkeye
*
bkx
=
new
MSKboundkeye
[
numvar
];
double
*
blx
=
new
double
[
numvar
];
double
*
bux
=
new
double
[
numvar
];
for
(
int
i
=
0
;
i
<
numvar
;
++
i
)
{
bkx
[
i
]
=
MSK_BK_FR
;
blx
[
i
]
=
-
MSK_INFINITY
;
bux
[
i
]
=
+
MSK_INFINITY
;
}
MSKrescodee
r
;
MSKtask_t
task
=
NULL
;
/* Create the optimization task. */
r
=
MSK_maketask
(
env_
,
numcon
,
numvar
,
&
task
);
/* Directs the log task stream to the 'printstr' function. */
/*if ( r==MSK_RES_OK )
r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/
/* Append 'numcon' empty constraints.
The constraints will initially have no bounds. */
if
(
r
==
MSK_RES_OK
)
{
r
=
MSK_appendcons
(
task
,
numcon
);
}
/* Append 'numvar' variables.
The variables will initially be fixed at zero (x=0). */
if
(
r
==
MSK_RES_OK
)
{
r
=
MSK_appendvars
(
task
,
numvar
);
}
for
(
int
j
=
0
;
j
<
numvar
&&
r
==
MSK_RES_OK
;
++
j
)
{
/* Set the linear term c_j in the objective.*/
if
(
r
==
MSK_RES_OK
)
{
r
=
MSK_putcj
(
task
,
j
,
0
);
}
/* Set the bounds on variable j.
blx[j] <= x_j <= bux[j] */
if
(
r
==
MSK_RES_OK
)
{
r
=
MSK_putvarbound
(
task
,
j
,
/* Index of variable.*/
bkx
[
j
],
/* Bound key.*/
blx
[
j
],
/* Numerical value of lower bound.*/
bux
[
j
]);
/* Numerical value of upper bound.*/
}
/* Input column j of A */
if
(
r
==
MSK_RES_OK
)
{
r
=
MSK_putacol
(
task
,
j
,
/* Variable (column) index.*/
aptre
[
j
]
-
aptrb
[
j
],
/* Number of non-zeros in column j.*/
asub
+
aptrb
[
j
],
/* Pointer to row indexes of column j.*/
aval
+
aptrb
[
j
]);
/* Pointer to Values of column j.*/
}
}
/* Set the bounds on constraints.
for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
for
(
int
i
=
0
;
i
<
numcon
&&
r
==
MSK_RES_OK
;
++
i
)
{
r
=
MSK_putconbound
(
task
,
i
,
/* Index of constraint.*/
bkc
[
i
],
/* Bound key.*/
blc
[
i
],
/* Numerical value of lower bound.*/