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
19cafdde
Commit
19cafdde
authored
Sep 03, 2019
by
Pierre Fernbach
Browse files
Merge remote-tracking branch 'jchemin/topic/add_piecewise_curve_final' into HEAD
parents
4d546b5e
dde8663f
Changes
14
Expand all
Hide whitespace changes
Inline
Side-by-side
include/curves/CMakeLists.txt
View file @
19cafdde
...
...
@@ -11,7 +11,7 @@ SET(${PROJECT_NAME}_HEADERS
quintic_spline.h
linear_variable.h
cubic_hermite_spline.h
piecewise_
polynomial_
curve.h
piecewise_curve.h
)
INSTALL
(
FILES
...
...
include/curves/MathDefs.h
View file @
19cafdde
...
...
@@ -26,20 +26,20 @@ namespace curves{
template
<
typename
_Matrix_Type_
>
void
PseudoInverse
(
_Matrix_Type_
&
pinvmat
)
{
Eigen
::
JacobiSVD
<
_Matrix_Type_
>
svd
(
pinvmat
,
Eigen
::
ComputeFullU
|
Eigen
::
ComputeFullV
);
_Matrix_Type_
m_sigma
=
svd
.
singularValues
();
Eigen
::
JacobiSVD
<
_Matrix_Type_
>
svd
(
pinvmat
,
Eigen
::
ComputeFullU
|
Eigen
::
ComputeFullV
);
_Matrix_Type_
m_sigma
=
svd
.
singularValues
();
double
pinvtoler
=
1.e-6
;
// choose your tolerance widely!
double
pinvtoler
=
1.e-6
;
// choose your tolerance widely!
_Matrix_Type_
m_sigma_inv
=
_Matrix_Type_
::
Zero
(
pinvmat
.
cols
(),
pinvmat
.
rows
());
for
(
long
i
=
0
;
i
<
m_sigma
.
rows
();
++
i
)
{
if
(
m_sigma
(
i
)
>
pinvtoler
)
{
m_sigma_inv
(
i
,
i
)
=
1.0
/
m_sigma
(
i
);
}
}
pinvmat
=
(
svd
.
matrixV
()
*
m_sigma_inv
*
svd
.
matrixU
().
transpose
());
_Matrix_Type_
m_sigma_inv
=
_Matrix_Type_
::
Zero
(
pinvmat
.
cols
(),
pinvmat
.
rows
());
for
(
long
i
=
0
;
i
<
m_sigma
.
rows
();
++
i
)
{
if
(
m_sigma
(
i
)
>
pinvtoler
)
{
m_sigma_inv
(
i
,
i
)
=
1.0
/
m_sigma
(
i
);
}
}
pinvmat
=
(
svd
.
matrixV
()
*
m_sigma_inv
*
svd
.
matrixU
().
transpose
());
}
}
// namespace curves
...
...
include/curves/bezier_curve.h
View file @
19cafdde
...
...
@@ -32,16 +32,16 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
bezier_curve
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
curve_constraints
<
point_t
>
curve_constraints_t
;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
typename
t_point_t
::
const_iterator
cit_point_t
;
typedef
bezier_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
bezier_curve_t
;
/* Constructors - destructors */
public:
public:
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
...
...
@@ -63,7 +63,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
1
||
T_max_
<=
T_min_
))
{
throw
std
::
out_of_range
(
"can't create bezier min bound is higher than max bound"
);
// TODO
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
// TODO
}
for
(;
it
!=
PointsEnd
;
++
it
)
{
...
...
@@ -90,7 +90,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if
(
Safe
&&
(
size_
<
1
||
T_max_
<=
T_min_
))
{
throw
std
::
out_of_range
(
"can't create bezier min bound is higher than max bound"
);
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
}
t_point_t
updatedList
=
add_constraints
<
In
>
(
PointsBegin
,
PointsEnd
,
constraints
);
for
(
cit_point_t
cit
=
updatedList
.
begin
();
cit
!=
updatedList
.
end
();
++
cit
)
...
...
@@ -99,27 +99,27 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
}
///\brief Destructor
~
bezier_curve
()
{
// NOTHING
}
///\brief Destructor
~
bezier_curve
()
{
// NOTHING
}
private:
//
bezier_curve(const bezier_curve&);
private:
//
bezier_curve(const bezier_curve&);
// bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
public:
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual
point_t
operator
()(
const
time_t
t
)
const
{
if
(
Safe
&!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
out_of_range
(
"can't evaluate bezier curve, out of range"
);
// TODO
throw
std
::
invalid_argument
(
"can't evaluate bezier curve,
time t is
out of range"
);
// TODO
}
if
(
size_
==
1
)
{
...
...
@@ -128,7 +128,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
return
evalHorner
(
t
);
}
}
}
/// \brief Compute the derived curve at order N.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
...
...
@@ -298,7 +298,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \return pair containing the first element of both bezier curve obtained.
///
std
::
pair
<
bezier_curve_t
,
bezier_curve_t
>
split
(
const
Numeric
t
){
if
(
t
==
T_max_
)
if
(
fabs
(
t
-
T_max_
)
<
MARGIN
)
{
throw
std
::
runtime_error
(
"can't split curve, interval range is equal to original curve"
);
}
...
...
@@ -326,15 +326,15 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
throw
std
::
out_of_range
(
"In Extract curve : times out of bounds"
);
}
if
(
t1
==
T_min_
&&
t2
==
T_max_
)
if
(
fabs
(
t1
-
T_min_
)
<
MARGIN
&&
fabs
(
t2
-
T_max_
)
<
MARGIN
)
{
return
bezier_curve_t
(
waypoints
().
begin
(),
waypoints
().
end
(),
T_min_
,
T_max_
,
mult_T_
);
}
if
(
t1
==
T_min_
)
if
(
fabs
(
t1
-
T_min_
)
<
MARGIN
)
{
return
split
(
t2
).
first
;
}
if
(
t2
==
T_max_
)
if
(
fabs
(
t2
-
T_max_
)
<
MARGIN
)
{
return
split
(
t1
).
second
;
}
...
...
@@ -384,7 +384,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
virtual
time_t
max
()
const
{
return
T_max_
;}
/*Helpers*/
public:
public:
/// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
/*const*/
time_t
T_min_
;
/// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
...
...
@@ -394,6 +394,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*const*/
std
::
size_t
degree_
;
/*const*/
std
::
vector
<
Bern
<
Numeric
>
>
bernstein_
;
/*const*/
t_point_t
control_points_
;
static
const
double
MARGIN
;
public:
static
bezier_curve_t
zero
(
const
time_t
T
=
1.
)
...
...
@@ -403,6 +404,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
return
bezier_curve_t
(
ts
.
begin
(),
ts
.
end
(),
0.
,
T
);
}
};
template
<
typename
Time
,
typename
Numeric
,
std
::
size_t
Dim
,
bool
Safe
,
typename
Point
>
const
double
bezier_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>::
MARGIN
(
0.001
);
}
// namespace curve
#endif //_CLASS_BEZIERCURVE
include/curves/cubic_hermite_spline.h
View file @
19cafdde
...
...
@@ -63,15 +63,15 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
/// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
/// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
/// \param time_control_points : vector containing time for each waypoint.
///
template
<
typename
In
>
cubic_hermite_spline
(
In
PairsBegin
,
In
PairsEnd
,
const
vector_time_t
&
time_control_points
)
{
// Check size of pairs container.
template
<
typename
In
>
cubic_hermite_spline
(
In
PairsBegin
,
In
PairsEnd
,
const
vector_time_t
&
time_control_points
)
{
// Check size of pairs container.
std
::
size_t
const
size
(
std
::
distance
(
PairsBegin
,
PairsEnd
));
size_
=
size
;
if
(
Safe
&&
size
<
1
)
...
...
@@ -85,30 +85,30 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
control_points_
.
push_back
(
*
it
);
}
setTime
(
time_control_points
);
}
}
/// \brief Destructor.
/// \brief Destructor.
virtual
~
cubic_hermite_spline
(){}
/*Operations*/
public:
public:
/// \brief Evaluation of the cubic hermite spline at time t.
/// \param t : time when to evaluate the spline.
/// \return \f$p(t)\f$ point corresponding on spline at time t.
///
virtual
Point
operator
()(
const
Time
t
)
const
virtual
Point
operator
()(
const
Time
t
)
const
{
if
(
Safe
&!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
out_of_range
(
"can't evaluate cubic hermite spline, out of range"
);
throw
std
::
invalid_argument
(
"can't evaluate cubic hermite spline, out of range"
);
}
if
(
size_
==
1
)
{
return
control_points_
.
front
().
first
;
return
control_points_
.
front
().
first
;
}
else
{
return
evalCubicHermiteSpline
(
t
,
0
);
return
evalCubicHermiteSpline
(
t
,
0
);
}
}
...
...
@@ -139,7 +139,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
computeDurationSplines
();
if
(
!
checkDurationSplines
())
{
throw
std
::
logic_error
(
"time_splines not monotonous, all spline duration should be superior to 0"
);
throw
std
::
invalid_argument
(
"time_splines not monotonous, all spline duration should be superior to 0"
);
}
}
...
...
@@ -214,7 +214,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
//
const
Time
dt
=
(
t1
-
t0
);
const
Time
alpha
=
(
t
-
t0
)
/
dt
;
assert
(
0.
<=
alpha
<=
1.
&&
"alpha must be in [0,1]"
);
assert
(
0.
<=
alpha
&&
alpha
<=
1.
&&
"alpha must be in [0,1]"
);
Numeric
h00
,
h10
,
h01
,
h11
;
evalCoeffs
(
alpha
,
h00
,
h10
,
h01
,
h11
,
order_derivative
);
//std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<" h11="<<h11<<std::endl;
...
...
@@ -353,17 +353,17 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
return
is_positive
;
}
/*Operations*/
/*Operations*/
/*Helpers*/
public:
public:
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range.
Time
virtual
min
()
const
{
return
time_control_points_
.
front
();}
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
Time
virtual
max
()
const
{
return
time_control_points_
.
back
();}
/*Helpers*/
/*Helpers*/
};
...
...
include/curves/curve_abc.h
View file @
19cafdde
...
...
@@ -25,23 +25,23 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
curve_abc
:
std
::
unary_function
<
Time
,
Point
>
{
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Point
point_t
;
typedef
Time
time_t
;
/* Constructors - destructors */
public:
/// \brief Constructor.
public:
/// \brief Constructor.
curve_abc
(){}
/// \brief Destructor.
/// \brief Destructor.
virtual
~
curve_abc
(){}
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : time when to evaluate the spine
/// \return \f$x(t)\f$, point corresponding on curve at time t.
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : time when to evaluate the spine
/// \return \f$x(t)\f$, point corresponding on curve at time t.
virtual
point_t
operator
()(
const
time_t
t
)
const
=
0
;
...
...
@@ -53,18 +53,18 @@ struct curve_abc : std::unary_function<Time, Point>
/*Operations*/
/*Helpers*/
public:
/// \brief Get the minimum time for which the curve is defined.
/// \return \f$t_{min}\f$, lower bound of time range.
virtual
time_t
min
()
const
=
0
;
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual
time_t
max
()
const
=
0
;
public:
/// \brief Get the minimum time for which the curve is defined.
/// \return \f$t_{min}\f$, lower bound of time range.
virtual
time_t
min
()
const
=
0
;
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual
time_t
max
()
const
=
0
;
std
::
pair
<
time_t
,
time_t
>
timeRange
()
{
return
std
::
make_pair
(
min
(),
max
());}
/*Helpers*/
};
};
}
// namespace curves
#endif //_STRUCT_CURVE_ABC
include/curves/curve_constraint.h
View file @
19cafdde
...
...
@@ -22,16 +22,16 @@ namespace curves
template
<
typename
Point
>
struct
curve_constraints
{
typedef
Point
point_t
;
typedef
Point
point_t
;
curve_constraints
()
:
init_vel
(
point_t
::
Zero
()),
init_acc
(
init_vel
),
end_vel
(
init_vel
),
end_acc
(
init_vel
){}
~
curve_constraints
(){}
~
curve_constraints
(){}
point_t
init_vel
;
point_t
init_acc
;
point_t
end_vel
;
point_t
end_acc
;
point_t
init_vel
;
point_t
init_acc
;
point_t
end_vel
;
point_t
end_acc
;
};
}
// namespace curves
#endif //_CLASS_CUBICZEROVELACC
include/curves/curve_conversion.h
View file @
19cafdde
...
...
@@ -38,7 +38,7 @@ Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
}
/// \brief Converts a cubic hermite spline or polynomial to a cubic bezier curve.
/// \brief Converts a cubic hermite spline or polynomial
of order 3 or less
to a cubic bezier curve.
/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic bezier curve.
template
<
typename
Bezier
,
typename
curveTypeToConvert
>
...
...
include/curves/exact_cubic.h
View file @
19cafdde
...
...
@@ -41,12 +41,12 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
,
typename
SplineBase
=
polynomial
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
>
struct
exact_cubic
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Eigen
::
Matrix
<
Numeric
,
3
,
3
>
Matrix3
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
SplineBase
spline_t
;
typedef
typename
std
::
vector
<
spline_t
>
t_spline_t
;
typedef
typename
t_spline_t
::
iterator
it_spline_t
;
...
...
@@ -54,14 +54,14 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
curve_abc_t
;
typedef
curve_constraints
<
point_t
>
spline_constraints
;
/* Constructors - destructors */
public:
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
/* Constructors - destructors */
public:
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
///
template
<
typename
In
>
exact_cubic
(
In
wayPointsBegin
,
In
wayPointsEnd
)
template
<
typename
In
>
exact_cubic
(
In
wayPointsBegin
,
In
wayPointsEnd
)
:
curve_abc_t
(),
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
{}
/// \brief Constructor.
...
...
@@ -82,7 +82,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic
(
const
exact_cubic
&
other
)
:
curve_abc_t
(),
subSplines_
(
other
.
subSplines_
)
{}
/// \brief Destructor.
/// \brief Destructor.
virtual
~
exact_cubic
(){}
std
::
size_t
getNumberSplines
()
...
...
@@ -249,13 +249,13 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
private:
//exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : time when to evaluate the spline
/// \return \f$x(t)\f$ point corresponding on spline at time t.
/// \return \f$x(t)\f$ point corresponding on spline at time t.
///
virtual
point_t
operator
()(
const
time_t
t
)
const
{
...
...
@@ -295,22 +295,22 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
// this should not happen
throw
std
::
runtime_error
(
"Exact cubic evaluation failed; t is outside bounds"
);
}
/*Operations*/
/*Operations*/
/*Helpers*/
public:
/*Helpers*/
public:
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$ lower bound of time range.
num_t
virtual
min
()
const
{
return
subSplines_
.
front
().
min
();}
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$ upper bound of time range.
num_t
virtual
max
()
const
{
return
subSplines_
.
back
().
max
();}
/*Helpers*/
/*Helpers*/
/*Attributes*/
/*Attributes*/
public:
t_spline_t
subSplines_
;
// const
/*Attributes*/
/*Attributes*/
};
}
// namespace curves
#endif //_CLASS_EXACTCUBIC
...
...
include/curves/linear_variable.h
View file @
19cafdde
...
...
@@ -50,7 +50,7 @@ struct linear_variable{
return
*
this
;
}
static
linear_variable_t
Zero
(
size_t
dim
=
0
){
static
linear_variable_t
Zero
(){
linear_variable_t
w
;
w
.
A_
=
matrix_t
::
Zero
();
w
.
b_
=
point_t
::
Zero
();
...
...
include/curves/piecewise_
polynomial_
curve.h
→
include/curves/piecewise_curve.h
View file @
19cafdde
/**
* \file piecewise_
polynomial_
curve.h
* \brief class allowing to create a piecewise
polynomial
curve.
* \file piecewise_curve.h
* \brief class allowing to create a piecewise curve.
* \author Jason C.
* \date 05/2019
*/
...
...
@@ -9,61 +9,60 @@
#define _CLASS_PIECEWISE_CURVE
#include
"curve_abc.h"
#include
"polynomial.h"
#include
"curve_conversion.h"
#include
<typeinfo>
namespace
curves
{
/// \class PiecewiseCurve.
/// \brief Represent a piecewise
polynomial
curve. We can add some new
polynomials to the
curve,
/// but the starting time of the
polynomial
to add should be equal to the ending time of the
/// piecewise_
polynomial_
curve.<br>\ Example : A piecewise
polynomial
curve composed of three
polynomials pol_
0,
///
pol_
1 and
pol_
2 where
pol_
0 is defined between \f$[T0_{min},T0_{max}]\f$,
pol_
1 between
/// \f$[T0_{max},T1_{max}]\f$ and
pol_
2 between \f$[T1_{max},T2_{max}]\f$.
/// On the piecewise polynomial curve,
pol_
0 is located between \f$[T0_{min},T0_{max}[\f$,
///
pol_
1 between \f$[T0_{max},T1_{max}[\f$ and
pol_
2 between \f$[T1_{max},T2_{max}]\f$.
/// \brief Represent a piecewise curve. We can add some new curve,
/// but the starting time of the
curve
to add should be equal to the ending time of the
actual
/// piecewise_curve.<br>\ Example : A piecewise curve composed of three
curves cf
0,
///
cf
1 and
cf
2 where
cf
0 is defined between \f$[T0_{min},T0_{max}]\f$,
cf
1 between
/// \f$[T0_{max},T1_{max}]\f$ and
cf
2 between \f$[T1_{max},T2_{max}]\f$.
/// On the piecewise polynomial curve,
cf
0 is located between \f$[T0_{min},T0_{max}[\f$,
///
cf
1 between \f$[T0_{max},T1_{max}[\f$ and
cf
2 between \f$[T1_{max},T2_{max}]\f$.
///
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
>
>
typename
T_Point
=
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
,
typename
Curve
=
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
>
struct
piecewise_
polynomial_
curve
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
struct
piecewise_curve
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
//typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
typedef
polynomial
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
>
polynomial_t
;
typedef
typename
std
::
vector
<
polynomial_t
>
t_polynomial_t
;
typedef
std
::
vector
<
Time
>
t_vector_time_t
;
public:
/// \brief Constructor.
/// Initialize a piecewise polynomial curve by giving the first polynomial curve.
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
Curve
curve_t
;
typedef
typename
std
::
vector
<
curve_t
>
t_curve_t
;
typedef
typename
std
::
vector
<
Time
>
t_time_t
;
public:
/// \brief Constructor.
/// Initialize a piecewise curve by giving the first curve.
/// \param pol : a polynomial curve.
///
piecewise_
polynomial_
curve
(
const
polynomial
_t
&
pol
)
{
size_
=
0
;
add_polynomial
_curve
(
pol
);
time_
polynomial_
curves_
.
push_back
(
pol
.
min
());
T_min_
=
pol
.
min
();
}
piecewise_curve
(
const
curve
_t
&
cf
)
{
size_
=
0
;
add
_curve
(
cf
);
time_curves_
.
push_back
(
cf
.
min
());
T_min_
=
cf
.
min
();
}
virtual
~
piecewise_
polynomial_
curve
(){}
virtual
~
piecewise_curve
(){}
virtual
Point
operator
()(
const
Time
t
)
const
virtual
Point
operator
()(
const
Time
t
)
const
{
if
(
Safe
&!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw
std
::
out_of_range
(
"can't evaluate piecewise curve, out of range"
);
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw
std
::
out_of_range
(
"can't evaluate piecewise curve, out of range"
);
}
return
polynomial_
curves_
.
at
(
find_interval
(
t
))(
t
);
return
curves_
.
at
(
find_interval
(
t
))(
t
);
}
/// \brief Evaluate the derivative of order N of curve at time t.
...
...
@@ -75,78 +74,78 @@ struct piecewise_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, P
{
if
(
Safe
&!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
out_of_range
(
"can't evaluate piecewise curve, out of range"
);
throw
std
::
invalid_argument
(
"can't evaluate piecewise curve, out of range"
);
}
return
(
polynomial_
curves_
.
at
(
find_interval
(
t
))).
derivate
(
t
,
order
);
return
(
curves_
.
at
(
find_interval
(
t
))).
derivate
(
t
,
order
);
}
/// \brief Add a new
polynomial
to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
/// \brief Add a new
curve
to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
/// is equal to \f$T_{max}\f$ of the actual piecewise curve.
/// \param
pol : polynomial
to add.
/// \param
cf : curve
to add.
///
void
add_
polynomial_curve
(
polynomial_t
pol
)
{
// Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
if
(
size_
!=
0
&&
pol
.
min
()
!=
T_max_
)
{
throw
std
::
out_of_range
(
"Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()"
);