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
Jason Chemin
curves
Commits
844e49b7
Commit
844e49b7
authored
Nov 25, 2016
by
Steve Tonneau
Browse files
externalized helper function to build cubic and quintic splines
parent
6ebf1138
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/spline/cubic_spline.h
View file @
844e49b7
...
...
@@ -33,6 +33,14 @@ T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point
res
.
push_back
(
a
);
res
.
push_back
(
b
);
res
.
push_back
(
c
);
res
.
push_back
(
d
);
return
res
;
}
template
<
typename
Time
,
typename
Numeric
,
std
::
size_t
Dim
,
bool
Safe
,
typename
Point
,
typename
T_Point
>
spline_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
create_cubic
(
Point
const
&
a
,
Point
const
&
b
,
Point
const
&
c
,
Point
const
&
d
,
const
Time
min
,
const
Time
max
)
{
T_Point
coeffs
=
make_cubic_vector
<
Point
,
T_Point
>
(
a
,
b
,
c
,
d
);
return
spline_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
coeffs
.
begin
(),
coeffs
.
end
(),
min
,
max
);
}
}
#endif //_STRUCT_CUBICSPLINE
include/spline/cubic_zero_vel_acc.h
View file @
844e49b7
...
...
@@ -20,8 +20,7 @@
#ifndef _CLASS_CUBICZEROVELACC
#define _CLASS_CUBICZEROVELACC
#include
"curve_abc.h"
#include
"cubic_spline.h"
#include
"exact_cubic.h"
#include
"MathDefs.h"
...
...
@@ -30,23 +29,28 @@
namespace
spline
{
/// \class
ExactCubic
/// \class
cubic_zero_vel.
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization
/// crossing each of the waypoint given in its initialization. Additional constraints
/// are used to increase the order of the last and first splines, to start and finish
/// trajectory with zero velocity and acceleration. Thus the first and last splines
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
exact_cubic
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>,
typename
T_Point
=
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
>
struct
cubic_zero_vel
:
public
exact_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
{
typedef
Point
point_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
cubic_spline
<
time_t
,
Numeric
,
Dim
,
Safe
,
Point
>
cubic_spline_t
;
typedef
quintic_spline
<
time_t
,
Numeric
,
Dim
,
Safe
,
Point
>
cubic_spline_t
;
typedef
typename
std
::
vector
<
cubic_spline_t
*>
T_cubic
;
typedef
typename
T_cubic
::
iterator
IT_cubic
;
typedef
typename
T_cubic
::
const_iterator
CIT_cubic
;
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
spline_curve
<
time_t
,
Numeric
,
Dim
,
Safe
,
point_t
,
t_point_t
>
spline_t
;
typedef
exact_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
exact_cubic_t
;
typedef
typename
std
::
vector
<
spline_t
>
t_spline_t
;
typedef
typename
t_spline_t
::
iterator
it_spline_t
;
typedef
typename
t_spline_t
::
const_iterator
cit_spline_t
;
/* Constructors - destructors */
public:
...
...
@@ -54,119 +58,19 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template
<
typename
In
>
exact_cubic
(
In
wayPointsBegin
,
In
wayPointsEnd
)
cubic_zero_vel
(
In
wayPointsBegin
,
In
wayPointsEnd
)
:
exact_cubic_t
(
wayPointsBegin
,
wayPointsEnd
)
{
std
::
size_t
const
size
(
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
));
if
(
Safe
&&
size
<
1
)
{
throw
;
// 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
);
MatrixX
h5
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h6
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
a
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
b
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
c
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
d
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
x
=
MatrixX
::
Zero
(
size
,
Dim
);
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
);
++
next
;
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
);
num_t
const
dTi_cube
(
dTi_sqr
*
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
;
h5
(
i
,
i
)
=
2
/
dTi_cube
;
h5
(
i
,
i
+
1
)
=
-
2
/
dTi_cube
;
h6
(
i
,
i
)
=
1
/
dTi_sqr
;
h6
(
i
,
i
+
1
)
=
1
/
dTi_sqr
;
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
;
}
x
.
row
(
i
)
=
(
*
it
).
second
.
transpose
();
}
// adding last x
x
.
row
(
size
-
1
)
=
(
*
it
).
second
.
transpose
();
a
=
x
;
PseudoInverse
(
h1
);
b
=
h1
*
h2
*
x
;
//h1 * b = h2 * x => b = (h1)^-1 * h2 * x
c
=
h3
*
x
+
h4
*
b
;
d
=
h5
*
x
+
h6
*
b
;
it
=
wayPointsBegin
,
next
=
wayPointsBegin
;
++
next
;
for
(
int
i
=
0
;
next
!=
wayPointsEnd
;
++
i
,
++
it
,
++
next
)
{
subSplines_
.
push_back
(
new
cubic_spline_t
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),
(
*
it
).
first
,
(
*
next
).
first
));
}
subSplines_
.
push_back
(
new
cubic_spline_t
(
a
.
row
(
size
-
1
),
b
.
row
(
size
-
1
),
c
.
row
(
size
-
1
),
d
.
row
(
size
-
1
),
(
*
it
).
first
,
(
*
it
).
first
));
// NOTHING
}
///\brief Destructor
~
exact_cubic
()
{
for
(
IT_cubic
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
{
delete
(
*
it
);
}
}
private:
exact_cubic
(
const
exact_cubic
&
);
exact_cubic
&
operator
=
(
const
exact_cubic
&
);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
{
if
(
Safe
&&
(
t
<
subSplines_
.
front
()
->
t_min_
||
t
>
subSplines_
.
back
()
->
t_max_
)){
throw
std
::
out_of_range
(
"TODO"
);}
for
(
CIT_cubic
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
{
if
(
t
>=
((
*
it
)
->
t_min_
)
&&
t
<=
((
*
it
)
->
t_max_
))
{
return
(
*
it
)
->
operator
()(
t
);
}
}
}
/*Operations*/
/*Helpers*/
public:
num_t
virtual
min
()
const
{
return
subSplines_
.
front
()
->
t_min_
;}
num_t
virtual
max
()
const
{
return
subSplines_
.
back
()
->
t_max_
;}
/*Helpers*/
~
cubic_zero_vel
(){}
/*Attributes*/
private:
T_cubic
subSplines_
;
/*Attributes*/
cubic_zero_vel
(
const
cubic_zero_vel
&
);
cubic_zero_vel
&
operator
=
(
const
cubic_zero_vel
&
);
/* Constructors - destructors */
};
}
#endif //_CLASS_CUBICZEROVELACC
...
...
include/spline/exact_cubic.h
View file @
844e49b7
...
...
@@ -48,6 +48,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef
typename
std
::
vector
<
spline_t
>
t_spline_t
;
typedef
typename
t_spline_t
::
iterator
it_spline_t
;
typedef
typename
t_spline_t
::
const_iterator
cit_spline_t
;
typedef
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
curve_abc_t
;
/* Constructors - destructors */
public:
...
...
@@ -56,7 +57,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template
<
typename
In
>
exact_cubic
(
In
wayPointsBegin
,
In
wayPointsEnd
)
:
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
:
curve_abc_t
(),
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
{
}
...
...
@@ -131,19 +132,14 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
it
=
wayPointsBegin
,
next
=
wayPointsBegin
;
++
next
;
for
(
int
i
=
0
;
next
!=
wayPointsEnd
;
++
i
,
++
it
,
++
next
)
{
add_cubic
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),(
*
it
).
first
,
(
*
next
).
first
,
subSplines
);
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),(
*
it
).
first
,
(
*
next
).
first
));
}
add_cubic
(
a
.
row
(
size
-
1
),
b
.
row
(
size
-
1
),
c
.
row
(
size
-
1
),
d
.
row
(
size
-
1
),(
*
it
).
first
,
(
*
it
).
first
,
subSplines
);
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
a
.
row
(
size
-
1
),
b
.
row
(
size
-
1
),
c
.
row
(
size
-
1
),
d
.
row
(
size
-
1
),
(
*
it
).
first
,
(
*
it
).
first
));
return
subSplines
;
}
void
add_cubic
(
point_t
const
&
a
,
point_t
const
&
b
,
point_t
const
&
c
,
point_t
const
&
d
,
const
time_t
min
,
const
time_t
max
,
t_spline_t
&
subSplines
)
const
{
t_point_t
coeffs
=
make_cubic_vector
<
point_t
,
t_point_t
>
(
a
,
b
,
c
,
d
);
subSplines
.
push_back
(
spline_t
(
coeffs
.
begin
(),
coeffs
.
end
(),
min
,
max
));
}
private:
exact_cubic
(
const
exact_cubic
&
);
exact_cubic
&
operator
=
(
const
exact_cubic
&
);
...
...
@@ -156,10 +152,10 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
{
if
(
Safe
&&
(
t
<
subSplines_
.
front
().
t_
min
_
||
t
>
subSplines_
.
back
().
t_
max
_
)){
throw
std
::
out_of_range
(
"TODO"
);}
if
(
Safe
&&
(
t
<
subSplines_
.
front
().
min
()
||
t
>
subSplines_
.
back
().
max
()
)){
throw
std
::
out_of_range
(
"TODO"
);}
for
(
cit_spline_t
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
{
if
(
t
>=
(
it
->
t_
min
_
)
&&
t
<=
(
it
->
t_
max
_
))
if
(
t
>=
(
it
->
min
()
)
&&
t
<=
(
it
->
max
()
))
{
return
it
->
operator
()(
t
);
}
...
...
@@ -169,8 +165,8 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
num_t
virtual
min
()
const
{
return
subSplines_
.
front
().
t_
min
_
;}
num_t
virtual
max
()
const
{
return
subSplines_
.
back
().
t_
max
_
;}
num_t
virtual
min
()
const
{
return
subSplines_
.
front
().
min
()
;}
num_t
virtual
max
()
const
{
return
subSplines_
.
back
().
max
()
;}
/*Helpers*/
/*Attributes*/
...
...
include/spline/quintic_spline.h
View file @
844e49b7
...
...
@@ -35,6 +35,14 @@ T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
res
.
push_back
(
d
);
res
.
push_back
(
e
);
res
.
push_back
(
f
);
return
res
;
}
template
<
typename
Time
,
typename
Numeric
,
std
::
size_t
Dim
,
bool
Safe
,
typename
Point
,
typename
T_Point
>
spline_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
create_quintic
(
Point
const
&
a
,
Point
const
&
b
,
Point
const
&
c
,
Point
const
&
d
,
Point
const
&
e
,
Point
const
&
f
,
const
Time
min
,
const
Time
max
)
{
T_Point
coeffs
=
make_quintic_vector
<
Point
,
T_Point
>
(
a
,
b
,
c
,
d
,
e
,
f
);
return
spline_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
coeffs
.
begin
(),
coeffs
.
end
(),
min
,
max
);
}
}
#endif //_STRUCT_QUINTIC_SPLINE
include/spline/spline_curve.h
View file @
844e49b7
...
...
@@ -39,6 +39,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef
typename
t_point_t
::
const_iterator
cit_point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
curve_abc_t
;
/* Constructors - destructors */
public:
///\brief Constructor
...
...
@@ -48,7 +49,8 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
spline_curve
(
const
T_Point
&
coefficients
,
const
time_t
min
,
const
time_t
max
)
:
coefficients_
(
coefficients
),
t_min_
(
min
),
t_max_
(
max
),
dim_
(
Dim
),
order_
(
coefficients_
.
size
()
+
1
)
:
curve_abc_t
(),
coefficients_
(
coefficients
),
t_min_
(
min
),
t_max_
(
max
),
dim_
(
Dim
),
order_
(
coefficients_
.
size
()
+
1
)
{
if
(
Safe
)
{
...
...
@@ -71,7 +73,8 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param max: UPPER bound on interval definition of the spline
template
<
typename
In
>
spline_curve
(
In
zeroOrderCoefficient
,
In
out
,
const
time_t
min
,
const
time_t
max
)
:
coefficients_
(
init_coeffs
(
zeroOrderCoefficient
,
out
)),
t_min_
(
min
),
t_max_
(
max
),
dim_
(
Dim
),
order_
(
coefficients_
.
size
()
+
1
)
:
coefficients_
(
init_coeffs
(
zeroOrderCoefficient
,
out
)),
dim_
(
Dim
),
order_
(
coefficients_
.
size
()
+
1
),
t_min_
(
min
),
t_max_
(
max
)
{
if
(
Safe
)
{
...
...
@@ -131,9 +134,11 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
t_point_t
coefficients_
;
time_t
t_min_
,
t_max_
;
std
::
size_t
dim_
;
std
::
size_t
order_
;
private:
time_t
t_min_
,
t_max_
;
/*Attributes*/
private:
...
...
src/tests/spline_test/Main.cpp
View file @
844e49b7
#include
"spline/exact_cubic.h"
#include
"spline/cubic_zero_vel_acc.h"
#include
"spline/bezier_curve.h"
#include
"spline/spline_curve.h"
...
...
@@ -14,7 +15,7 @@ namespace spline
typedef
Eigen
::
Vector3d
point_t
;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
spline_curve
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
>
cubic_function_t
;
typedef
exact_cubic
<
double
,
double
,
3
,
true
,
point_t
>
exact_cubic_t
;
typedef
exact_cubic
<
double
,
double
,
3
,
true
,
point_t
>
exact_cubic_t
;
typedef
bezier_curve
<
double
,
double
,
3
,
true
,
point_t
>
bezier_curve_t
;
typedef
std
::
pair
<
double
,
point_t
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment