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
Pierre Fernbach
curves
Commits
ea0f5767
Commit
ea0f5767
authored
Sep 23, 2019
by
Pierre Fernbach
Browse files
Merge branch 'topic/bindingPythonDynamicCurves' into devel
parents
a9efbf46
cf72bc1b
Changes
17
Expand all
Hide whitespace changes
Inline
Side-by-side
include/curves/bezier_curve.h
View file @
ea0f5767
...
...
@@ -28,7 +28,7 @@ namespace curves
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>
>
struct
bezier_curve
:
public
curve_abc
<
Time
,
Numeric
,
Safe
,
Point
>
{
...
...
@@ -38,14 +38,14 @@ namespace curves
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
;
typedef
bezier_curve
<
Time
,
Numeric
,
Safe
,
Point
>
bezier_curve_t
;
/* Constructors - destructors */
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
bezier_curve
()
:
T_min_
(
0
),
T_max_
(
0
)
:
dim_
(
0
),
T_min_
(
0
),
T_max_
(
0
)
{}
/// \brief Constructor.
...
...
@@ -74,6 +74,11 @@ namespace curves
{
control_points_
.
push_back
(
*
it
);
}
// set dim
if
(
control_points_
.
size
()
!=
0
)
{
dim_
=
PointsBegin
->
size
();
}
}
/// \brief Constructor
...
...
@@ -102,10 +107,15 @@ namespace curves
{
control_points_
.
push_back
(
*
cit
);
}
// set dim
if
(
control_points_
.
size
()
!=
0
)
{
dim_
=
PointsBegin
->
size
();
}
}
bezier_curve
(
const
bezier_curve
&
other
)
:
T_min_
(
other
.
T_min_
),
T_max_
(
other
.
T_max_
),
:
dim_
(
other
.
dim_
),
T_min_
(
other
.
T_min_
),
T_max_
(
other
.
T_max_
),
mult_T_
(
other
.
mult_T_
),
size_
(
other
.
size_
),
degree_
(
other
.
degree_
),
bernstein_
(
other
.
bernstein_
),
control_points_
(
other
.
control_points_
)
...
...
@@ -123,7 +133,7 @@ namespace curves
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual
point_t
operator
()(
const
time_t
t
)
const
{
check_
if_not_empty
();
check_
conditions
();
if
(
Safe
&!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
invalid_argument
(
"can't evaluate bezier curve, time t is out of range"
);
// TODO
...
...
@@ -144,7 +154,7 @@ namespace curves
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t
compute_derivate
(
const
std
::
size_t
order
)
const
{
check_
if_not_empty
();
check_
conditions
();
if
(
order
==
0
)
{
return
*
this
;
...
...
@@ -156,7 +166,7 @@ namespace curves
}
if
(
derived_wp
.
empty
())
{
derived_wp
.
push_back
(
point_t
::
Zero
(
D
im
));
derived_wp
.
push_back
(
point_t
::
Zero
(
d
im
_
));
}
bezier_curve_t
deriv
(
derived_wp
.
begin
(),
derived_wp
.
end
(),
T_min_
,
T_max_
,
mult_T_
*
(
1.
/
(
T_max_
-
T_min_
))
);
return
deriv
.
compute_derivate
(
order
-
1
);
...
...
@@ -169,14 +179,14 @@ namespace curves
/// \return primitive at order N of x(t).
bezier_curve_t
compute_primitive
(
const
std
::
size_t
order
)
const
{
check_
if_not_empty
();
check_
conditions
();
if
(
order
==
0
)
{
return
*
this
;
}
num_t
new_degree
=
(
num_t
)(
degree_
+
1
);
t_point_t
n_wp
;
point_t
current_sum
=
point_t
::
Zero
(
D
im
);
point_t
current_sum
=
point_t
::
Zero
(
d
im
_
);
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
// then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
n_wp
.
push_back
(
current_sum
);
...
...
@@ -213,7 +223,7 @@ namespace curves
point_t
evalBernstein
(
const
Numeric
t
)
const
{
const
Numeric
u
=
(
t
-
T_min_
)
/
(
T_max_
-
T_min_
);
point_t
res
=
point_t
::
Zero
(
D
im
);
point_t
res
=
point_t
::
Zero
(
d
im
_
);
typename
t_point_t
::
const_iterator
control_points_it
=
control_points_
.
begin
();
for
(
typename
std
::
vector
<
Bern
<
Numeric
>
>::
const_iterator
cit
=
bernstein_
.
begin
();
cit
!=
bernstein_
.
end
();
++
cit
,
++
control_points_it
)
...
...
@@ -316,7 +326,7 @@ namespace curves
///
std
::
pair
<
bezier_curve_t
,
bezier_curve_t
>
split
(
const
Numeric
t
)
{
check_
if_not_empty
();
check_
conditions
();
if
(
fabs
(
t
-
T_max_
)
<
MARGIN
)
{
throw
std
::
runtime_error
(
"can't split curve, interval range is equal to original curve"
);
...
...
@@ -388,17 +398,24 @@ namespace curves
return
res
;
}
void
check_
if_not_empty
()
const
void
check_
conditions
()
const
{
if
(
control_points_
.
size
()
==
0
)
{
throw
std
::
runtime_error
(
"Error in beziercurve : there is no control points set / did you use empty constructor ?"
);
throw
std
::
runtime_error
(
"Error in bezier curve : there is no control points set / did you use empty constructor ?"
);
}
else
if
(
dim_
==
0
)
{
throw
std
::
runtime_error
(
"Error in bezier curve : Dimension of points is zero / did you use empty constructor ?"
);
}
}
/*Operations*/
public:
/*Helpers*/
/// \brief Get dimension of curve.
/// \return dimension of curve.
std
::
size_t
virtual
dim
()
const
{
return
dim_
;};
/// \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
{
return
T_min_
;}
...
...
@@ -408,6 +425,8 @@ namespace curves
/*Helpers*/
/* Attributes */
/// Dim of curve
std
::
size_t
dim_
;
/// 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.
...
...
@@ -420,10 +439,10 @@ namespace curves
static
const
double
MARGIN
;
/* Attributes */
static
bezier_curve_t
zero
(
const
time_t
T
=
1.
)
static
bezier_curve_t
zero
(
const
std
::
size_t
dim
,
const
time_t
T
=
1.
)
{
std
::
vector
<
point_t
>
ts
;
ts
.
push_back
(
point_t
::
Zero
(
D
im
));
ts
.
push_back
(
point_t
::
Zero
(
d
im
));
return
bezier_curve_t
(
ts
.
begin
(),
ts
.
end
(),
0.
,
T
);
}
...
...
@@ -435,6 +454,7 @@ namespace curves
if
(
version
)
{
// Do something depending on version ?
}
ar
&
boost
::
serialization
::
make_nvp
(
"dim"
,
dim_
);
ar
&
boost
::
serialization
::
make_nvp
(
"T_min"
,
T_min_
);
ar
&
boost
::
serialization
::
make_nvp
(
"T_max"
,
T_max_
);
ar
&
boost
::
serialization
::
make_nvp
(
"mult_T"
,
mult_T_
);
...
...
@@ -445,8 +465,8 @@ namespace curves
}
};
// End struct bezier_curve
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
);
template
<
typename
Time
,
typename
Numeric
,
bool
Safe
,
typename
Point
>
const
double
bezier_curve
<
Time
,
Numeric
,
Safe
,
Point
>::
MARGIN
(
0.001
);
}
// namespace curve
#endif //_CLASS_BEZIERCURVE
...
...
include/curves/cubic_hermite_spline.h
View file @
ea0f5767
...
...
@@ -30,12 +30,11 @@ namespace curves
/// - crosses each of the waypoint given in its initialization (\f$P_0\f$, \f$P_1\f$,...,\f$P_N\f$).
/// - has its derivatives on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$p'(t_{P_i}) = m_i\f$ and \f$p'(t_{P_{i+1}}) = m_{i+1}\f$.
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>,
typename
Tangent
=
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>
>
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>
>
struct
cubic_hermite_spline
:
public
curve_abc
<
Time
,
Numeric
,
Safe
,
Point
>
{
typedef
std
::
pair
<
Point
,
Tange
nt
>
pair_point_tangent_t
;
typedef
std
::
pair
<
Point
,
Poi
nt
>
pair_point_tangent_t
;
typedef
std
::
vector
<
pair_point_tangent_t
,
Eigen
::
aligned_allocator
<
Point
>
>
t_pair_point_tangent_t
;
typedef
std
::
vector
<
Time
>
vector_time_t
;
typedef
Numeric
num_t
;
...
...
@@ -44,7 +43,7 @@ namespace curves
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
cubic_hermite_spline
()
:
T_min_
(
0
),
T_max_
(
0
)
:
dim_
(
0
),
T_min_
(
0
),
T_max_
(
0
)
{}
/// \brief Constructor.
...
...
@@ -67,12 +66,18 @@ namespace curves
{
control_points_
.
push_back
(
*
it
);
}
// Set dimension according to size of points
if
(
control_points_
.
size
()
!=
0
)
{
dim_
=
control_points_
[
0
].
first
.
size
();
}
// Set time
setTime
(
time_control_points
);
}
cubic_hermite_spline
(
const
cubic_hermite_spline
&
other
)
:
control_points_
(
other
.
control_points_
),
time_control_points_
(
other
.
time_control_points_
),
:
dim_
(
other
.
dim_
),
control_points_
(
other
.
control_points_
),
time_control_points_
(
other
.
time_control_points_
),
duration_splines_
(
other
.
duration_splines_
),
T_min_
(
other
.
T_min_
),
T_max_
(
other
.
T_max_
),
size_
(
other
.
size_
),
degree_
(
other
.
degree_
)
{}
...
...
@@ -89,7 +94,7 @@ namespace curves
///
virtual
Point
operator
()(
const
Time
t
)
const
{
check_
if_not_empty
();
check_
conditions
();
if
(
Safe
&!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
invalid_argument
(
"can't evaluate cubic hermite spline, out of range"
);
...
...
@@ -111,7 +116,7 @@ namespace curves
///
virtual
Point
derivate
(
const
Time
t
,
const
std
::
size_t
order
)
const
{
check_
if_not_empty
();
check_
conditions
();
return
evalCubicHermiteSpline
(
t
,
order
);
}
...
...
@@ -315,12 +320,16 @@ namespace curves
return
left_id
-
1
;
}
void
check_
if_not_empty
()
const
void
check_
conditions
()
const
{
if
(
control_points_
.
size
()
==
0
)
{
throw
std
::
runtime_error
(
"Error in cubic hermite : there is no control points set / did you use empty constructor ?"
);
}
else
if
(
dim_
==
0
)
{
throw
std
::
runtime_error
(
"Error in cubic hermite : Dimension of points is zero / did you use empty constructor ?"
);
}
}
/// \brief compute duration of each spline.
...
...
@@ -358,6 +367,9 @@ namespace curves
/*Helpers*/
public:
/// \brief Get dimension of curve.
/// \return dimension of curve.
std
::
size_t
virtual
dim
()
const
{
return
dim_
;};
/// \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
();}
...
...
@@ -367,12 +379,13 @@ namespace curves
/*Helpers*/
/*Attributes*/
/// Dim of curve
std
::
size_t
dim_
;
/// Vector of pair < Point, Tangent >.
t_pair_point_tangent_t
control_points_
;
/// Vector of Time corresponding to time of each N control points : time at \f$P_0, P_1, P_2, ..., P_N\f$.
/// Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.
vector_time_t
time_control_points_
;
/// Vector of Time corresponding to time duration of each subspline.<br>
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
/// duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br>
...
...
@@ -396,6 +409,7 @@ namespace curves
if
(
version
)
{
// Do something depending on version ?
}
ar
&
boost
::
serialization
::
make_nvp
(
"dim"
,
dim_
);
ar
&
boost
::
serialization
::
make_nvp
(
"control_points"
,
control_points_
);
ar
&
boost
::
serialization
::
make_nvp
(
"time_control_points"
,
time_control_points_
);
ar
&
boost
::
serialization
::
make_nvp
(
"duration_splines"
,
duration_splines_
);
...
...
include/curves/cubic_spline.h
View file @
ea0f5767
...
...
@@ -35,13 +35,12 @@ namespace curves
return
res
;
}
template
<
typename
Time
,
typename
Numeric
,
std
::
size_t
Dim
,
bool
Safe
,
typename
Point
,
typename
T_Point
>
polynomial
<
Time
,
Numeric
,
Dim
,
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
)
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
,
Dim
,
Safe
,
Point
,
T_Point
>
(
coeffs
.
begin
(),
coeffs
.
end
(),
t_min
,
t_max
);
return
polynomial
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
(
coeffs
.
begin
(),
coeffs
.
end
(),
t_min
,
t_max
);
}
}
// namespace curves
#endif //_STRUCT_CUBICSPLINE
...
...
include/curves/curve_abc.h
View file @
ea0f5767
...
...
@@ -54,6 +54,9 @@ namespace curves
/*Operations*/
/*Helpers*/
/// \brief Get dimension of curve.
/// \return dimension of curve.
virtual
std
::
size_t
dim
()
const
=
0
;
/// \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
;
...
...
include/curves/curve_constraint.h
View file @
ea0f5767
...
...
@@ -19,12 +19,11 @@
namespace
curves
{
template
<
typename
Point
,
std
::
size_t
Dim
=
3
>
template
<
typename
Point
>
struct
curve_constraints
{
typedef
Point
point_t
;
curve_constraints
()
:
init_vel
(
point_t
::
Zero
(
Dim
)),
init_acc
(
init_vel
),
end_vel
(
init_vel
),
end_acc
(
init_vel
){}
curve_constraints
(){}
~
curve_constraints
(){}
...
...
include/curves/exact_cubic.h
View file @
ea0f5767
...
...
@@ -37,11 +37,11 @@ namespace curves
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization.
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>,
typename
T_Point
=
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
,
typename
SplineBase
=
polynomial
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
>
struct
exact_cubic
:
public
piecewise_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
,
SplineBase
>
typename
SplineBase
=
polynomial
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
>
struct
exact_cubic
:
public
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
,
SplineBase
>
{
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
...
...
@@ -53,10 +53,10 @@ namespace curves
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_constraints
<
Point
,
Dim
>
spline_constraints
;
typedef
curve_constraints
<
Point
>
spline_constraints
;
typedef
exact_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
,
SplineBase
>
exact_cubic_t
;
typedef
piecewise_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
,
SplineBase
>
piecewise_curve_t
;
typedef
exact_cubic
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
,
SplineBase
>
exact_cubic_t
;
typedef
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
,
SplineBase
>
piecewise_curve_t
;
/* Constructors - destructors */
public:
...
...
@@ -119,7 +119,8 @@ namespace curves
template
<
typename
In
>
t_spline_t
computeWayPoints
(
In
wayPointsBegin
,
In
wayPointsEnd
)
const
{
std
::
size_t
const
size
(
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
));
const
std
::
size_t
dim
=
wayPointsBegin
->
second
.
size
();
const
std
::
size_t
size
=
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
);
if
(
Safe
&&
size
<
1
)
{
throw
std
::
length_error
(
"size of waypoints must be superior to 0"
)
;
// TODO
...
...
@@ -132,11 +133,11 @@ namespace curves
MatrixX
h4
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h5
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h6
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
a
=
MatrixX
::
Zero
(
size
,
D
im
);
MatrixX
b
=
MatrixX
::
Zero
(
size
,
D
im
);
MatrixX
c
=
MatrixX
::
Zero
(
size
,
D
im
);
MatrixX
d
=
MatrixX
::
Zero
(
size
,
D
im
);
MatrixX
x
=
MatrixX
::
Zero
(
size
,
D
im
);
MatrixX
a
=
MatrixX
::
Zero
(
size
,
d
im
);
MatrixX
b
=
MatrixX
::
Zero
(
size
,
d
im
);
MatrixX
c
=
MatrixX
::
Zero
(
size
,
d
im
);
MatrixX
d
=
MatrixX
::
Zero
(
size
,
d
im
);
MatrixX
x
=
MatrixX
::
Zero
(
size
,
d
im
);
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
);
++
next
;
// Fill the matrices H as specified in the paper.
...
...
@@ -182,9 +183,9 @@ namespace curves
for
(
int
i
=
0
;
next
!=
wayPointsEnd
;
++
i
,
++
it
,
++
next
)
{
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
));
create_cubic
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),
(
*
it
).
first
,
(
*
next
).
first
));
}
return
subSplines
;
}
...
...
@@ -218,7 +219,7 @@ namespace curves
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
,
Dim
,
Safe
,
Point
,
T_Point
>
(
a0
,
b0
,
c0
,
d0
,
init_t
,
end_t
));
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
(
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
);
}
...
...
@@ -226,9 +227,10 @@ namespace curves
template
<
typename
In
>
void
compute_end_spline
(
In
wayPointsBegin
,
In
wayPointsNext
,
spline_constraints
&
constraints
,
t_spline_t
&
subSplines
)
const
{
const
std
::
size_t
dim
=
wayPointsBegin
->
second
.
size
();
const
point_t
&
a0
=
wayPointsBegin
->
second
,
a1
=
wayPointsNext
->
second
;
const
point_t
&
b0
=
constraints
.
init_vel
,
b1
=
constraints
.
end_vel
,
c0
=
constraints
.
init_acc
/
2.
,
c1
=
constraints
.
end_acc
;
c0
=
constraints
.
init_acc
/
2.
,
c1
=
constraints
.
end_acc
;
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
,
dt_4
=
dt_3
*
dt
,
dt_5
=
dt_4
*
dt
;
//solving a system of four linear eq with 4 unknows: d0, e0
...
...
@@ -239,7 +241,7 @@ namespace curves
const
num_t
x_e_0
=
dt_4
,
x_e_1
=
4
*
dt_3
,
x_e_2
=
12
*
dt_2
;
const
num_t
x_f_0
=
dt_5
,
x_f_1
=
5
*
dt_4
,
x_f_2
=
20
*
dt_3
;
point_t
d
,
e
,
f
;
MatrixX
rhs
=
MatrixX
::
Zero
(
3
,
D
im
);
MatrixX
rhs
=
MatrixX
::
Zero
(
3
,
d
im
);
rhs
.
row
(
0
)
=
alpha_0
;
rhs
.
row
(
1
)
=
alpha_1
;
rhs
.
row
(
2
)
=
alpha_2
;
Matrix3
eq
=
Matrix3
::
Zero
(
3
,
3
);
eq
(
0
,
0
)
=
x_d_0
;
eq
(
0
,
1
)
=
x_e_0
;
eq
(
0
,
2
)
=
x_f_0
;
...
...
@@ -247,7 +249,7 @@ namespace curves
eq
(
2
,
0
)
=
x_d_2
;
eq
(
2
,
1
)
=
x_e_2
;
eq
(
2
,
2
)
=
x_f_2
;
rhs
=
eq
.
inverse
().
eval
()
*
rhs
;
d
=
rhs
.
row
(
0
);
e
=
rhs
.
row
(
1
);
f
=
rhs
.
row
(
2
);
subSplines
.
push_back
(
create_quintic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
a0
,
b0
,
c0
,
d
,
e
,
f
,
init_t
,
end_t
));
subSplines
.
push_back
(
create_quintic
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
>
(
a0
,
b0
,
c0
,
d
,
e
,
f
,
init_t
,
end_t
));
}
public:
...
...
include/curves/helpers/effector_spline.h
View file @
ea0f5767
...
...
@@ -28,11 +28,11 @@ namespace curves
{
typedef
double
Numeric
;
typedef
double
Time
;
typedef
Eigen
::
Matrix
<
Numeric
,
3
,
1
>
Point
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>
Point
;
typedef
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
T_Point
;
typedef
std
::
pair
<
double
,
Point
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
typedef
exact_cubic
<
Time
,
Numeric
,
3
,
true
,
Point
,
T_Point
>
exact_cubic_t
;
typedef
exact_cubic
<
Time
,
Numeric
,
true
,
Point
,
T_Point
>
exact_cubic_t
;
typedef
exact_cubic_t
::
spline_constraints
spline_constraints_t
;
typedef
exact_cubic_t
::
t_spline_t
t_spline_t
;
typedef
exact_cubic_t
::
spline_t
spline_t
;
...
...
@@ -68,6 +68,8 @@ namespace curves
spline_constraints_t
compute_required_offset_velocity_acceleration
(
const
spline_t
&
end_spline
,
const
Time
/*time_offset*/
)
{
spline_constraints_t
constraints
;
constraints
.
init_acc
=
Point
::
Zero
(
end_spline
.
dim
());
constraints
.
init_vel
=
Point
::
Zero
(
end_spline
.
dim
());
constraints
.
end_acc
=
end_spline
.
derivate
(
end_spline
.
min
(),
2
);
constraints
.
end_vel
=
end_spline
.
derivate
(
end_spline
.
min
(),
1
);
return
constraints
;
...
...
include/curves/helpers/effector_spline_rotation.h
View file @
ea0f5767
...
...
@@ -36,7 +36,7 @@ namespace curves
typedef
std
::
pair
<
Numeric
,
quat_t
>
waypoint_quat_t
;
typedef
std
::
vector
<
waypoint_quat_t
>
t_waypoint_quat_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
1
,
1
>
point_one_dim_t
;
typedef
exact_cubic
<
Numeric
,
Numeric
,
1
,
false
,
point_one_dim_t
>
exact_cubic_constraint_one_dim
;
typedef
exact_cubic
<
Numeric
,
Numeric
,
false
,
point_one_dim_t
>
exact_cubic_constraint_one_dim
;
typedef
std
::
pair
<
Numeric
,
point_one_dim_t
>
waypoint_one_dim_t
;
typedef
std
::
vector
<
waypoint_one_dim_t
>
t_waypoint_one_dim_t
;
...
...
@@ -87,10 +87,12 @@ namespace curves
return
exact_cubic_constraint_one_dim
(
waypoints
.
begin
(),
waypoints
.
end
());
}
virtual
std
::
size_t
dim
()
const
{
return
dim_
;}
virtual
time_t
min
()
const
{
return
min_
;}
virtual
time_t
max
()
const
{
return
max_
;}
/*Attributes*/
std
::
size_t
dim_
;
//const
Eigen
::
Quaterniond
quat_from_
;
//const
Eigen
::
Quaterniond
quat_to_
;
//const
double
min_
;
//const
...
...
@@ -100,7 +102,7 @@ namespace curves
};
// End class rotation_spline
typedef
exact_cubic
<
Time
,
Numeric
,
4
,
false
,
quat_t
,
std
::
vector
<
quat_t
,
Eigen
::
aligned_allocator
<
quat_t
>
>
,
rotation_spline
>
exact_cubic_quat_t
;
typedef
exact_cubic
<
Time
,
Numeric
,
false
,
quat_t
,
std
::
vector
<
quat_t
,
Eigen
::
aligned_allocator
<
quat_t
>
>
,
rotation_spline
>
exact_cubic_quat_t
;
/// \class effector_spline_rotation.
/// \brief Represents a trajectory for and end effector.
...
...
include/curves/linear_variable.h
View file @
ea0f5767
...
...
@@ -116,6 +116,12 @@ namespace curves
return
*
this
;
}
std
::
size_t
size
()
{
variables_t
w
;
return
w
.
size
();
}
static
variables_t
Zero
(
size_t
/*dim*/
){
variables_t
w
;
return
w
;
...
...
include/curves/piecewise_curve.h
View file @
ea0f5767
...
...
@@ -22,7 +22,7 @@ namespace curves
/// On the piecewise polynomial curve, cf0 is located between \f$[T0_{min},T0_{max}[\f$,
/// cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>,
typename
T_Point
=
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
,
typename
Curve
=
curve_abc
<
Time
,
Numeric
,
Safe
,
Point
>
>
...
...
@@ -40,7 +40,7 @@ namespace curves
/// \brief Empty constructor. Add at least one curve to call other class functions.
///
piecewise_curve
()
:
size_
(
0
),
T_min_
(
0
),
T_max_
(
0
)
:
dim_
(
0
),
size_
(
0
),
T_min_
(
0
),
T_max_
(
0
)
{}
/// \brief Constructor.
...
...
@@ -49,12 +49,17 @@ namespace curves
///
piecewise_curve
(
const
curve_t
&
cf
)
{
dim_
=
cf
(
cf
.
min
()).
size
();
size_
=
0
;
add_curve
(
cf
);
}
piecewise_curve
(
const
t_curve_t
list_curves
)
{
if
(
list_curves
.
size
()
!=
0
)
{
dim_
=
list_curves
[
0
](
list_curves
[
0
].
min
()).
size
();
}
size_
=
0
;
for
(
std
::
size_t
i
=
0
;
i
<
list_curves
.
size
();
i
++
)
{
...
...
@@ -63,7 +68,7 @@ namespace curves
}
piecewise_curve
(
const
piecewise_curve
&
other
)
:
curves_
(
other
.
curves_
),
time_curves_
(
other
.
time_curves_
),
size_
(
other
.
size_
),
:
dim_
(
other
.
dim_
),
curves_
(
other
.
curves_
),
time_curves_
(
other
.
time_curves_
),
size_
(
other
.
size_
),
T_min_
(
other
.
T_min_
),
T_max_
(
other
.
T_max_
)
{}
...
...
@@ -102,6 +107,10 @@ namespace curves
///
void
add_curve
(
const
curve_t
&
cf
)
{
if
(
size_
==
0
&&
dim_
==
0
)
{
dim_
=
cf
(
cf
.
min
()).
size
();
}
// Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
if
(
size_
!=
0
&&
!
(
fabs
(
cf
.
min
()
-
T_max_
)
<
MARGIN
))
{
...
...
@@ -153,10 +162,10 @@ namespace curves
}
template
<
typename
Bezier
>
piecewise_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
,
Bezier
>
convert_piecewise_curve_to_bezier
()
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
,
Bezier
>
convert_piecewise_curve_to_bezier
()
{
check_if_not_empty
();
typedef
piecewise_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
,
Bezier
>
piecewise_curve_out_t
;
typedef
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
,
Bezier
>
piecewise_curve_out_t
;
// Get first curve (segment)
curve_t
first_curve
=
curves_
.
at
(
0
);
Bezier
first_curve_output
=
bezier_from_curve
<
Bezier
,
curve_t
>
(
first_curve
);
...
...
@@ -171,10 +180,10 @@ namespace curves
}
template
<
typename
Hermite
>
piecewise_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
,
Hermite
>
convert_piecewise_curve_to_cubic_hermite
()
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
,
Hermite
>
convert_piecewise_curve_to_cubic_hermite
()
{
check_if_not_empty
();
typedef
piecewise_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
,
Hermite
>
piecewise_curve_out_t
;
typedef
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
,
Hermite
>
piecewise_curve_out_t
;
// Get first curve (segment)
curve_t
first_curve
=
curves_
.
at
(
0
);
Hermite
first_curve_output
=
hermite_from_curve
<
Hermite
,
curve_t
>
(
first_curve
);
...
...
@@ -189,10 +198,10 @@ namespace curves
}
template
<
typename
Polynomial
>
piecewise_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
,
Polynomial
>
convert_piecewise_curve_to_polynomial
()
piecewise_curve
<
Time
,
Numeric
,
Safe
,
Point
,
T_Point
,
Polynomial
>
convert_piecewise_curve_to_polynomial
()
{