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
Jason Chemin
curves
Commits
e4131820
Commit
e4131820
authored
Apr 05, 2017
by
Steve Tonneau
Browse files
Merge branch 'packaging'
parents
efa985d6
4b1c4e8e
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/spline/bezier_curve.h
View file @
e4131820
...
...
@@ -12,6 +12,7 @@
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
...
...
@@ -34,7 +35,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
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 */
...
...
@@ -49,18 +52,36 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
))
,
degree_
(
size_
-
1
)
,
bernstein_
(
spline
::
makeBernstein
<
num_t
>
(
degree_
))
{
{
assert
(
bernstein_
.
size
()
==
size_
);
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
1
||
minBound
>=
maxBound
))
{
throw
std
::
out_of_range
(
"can't create bezier min bound is higher than max bound"
);
// TODO
}
for
(;
it
!=
PointsEnd
;
++
it
)
{
pts_
.
push_back
(
*
it
);
}
}
for
(;
it
!=
PointsEnd
;
++
it
)
pts_
.
push_back
(
*
it
);
}
///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///\param constraints : constraints applying on start / end velocities and acceleration
///
template
<
typename
In
>
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
curve_constraints_t
&
constraints
,
const
time_t
minBound
=
0
,
const
time_t
maxBound
=
1
)
:
minBound_
(
minBound
)
,
maxBound_
(
maxBound
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
)
+
4
)
,
degree_
(
size_
-
1
)
,
bernstein_
(
spline
::
makeBernstein
<
num_t
>
(
degree_
))
{
if
(
Safe
&&
(
size_
<
1
||
minBound
>=
maxBound
))
throw
std
::
out_of_range
(
"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
)
pts_
.
push_back
(
*
cit
);
}
///\brief Destructor
~
bezier_curve
()
...
...
@@ -86,8 +107,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
throw
std
::
out_of_range
(
"can't evaluate bezier curve, out of range"
);
// TODO
}
else
{
num_t
dt
=
(
1
-
nT
);
{
num_t
dt
=
(
1
-
nT
);
switch
(
size_
)
{
case
1
:
...
...
@@ -106,7 +127,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
+
3
*
pts_
[
2
]
*
nT
*
nT
*
dt
+
pts_
[
3
]
*
nT
*
nT
*
nT
;
default
:
return
evalBernstein
(
dt
);
return
evalBernstein
(
nT
);
break
;
}
}
...
...
@@ -154,7 +175,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param order : order of the derivative
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
virtual
point_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
{
bezier_curve_t
deriv
=
compute_derivate
(
order
);
return
deriv
(
t
);
...
...
@@ -169,17 +190,41 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typename
t_point_t
::
const_iterator
pts_it
=
pts_
.
begin
();
for
(
typename
std
::
vector
<
Bern
<
Numeric
>
>::
const_iterator
cit
=
bernstein_
.
begin
();
cit
!=
bernstein_
.
end
();
++
cit
,
++
pts_it
)
{
res
+=
cit
->
operator
()(
u
)
*
(
*
pts_it
);
}
return
res
;
}
const
t_point_t
&
waypoints
()
const
{
return
pts_
;}
private:
template
<
typename
In
>
t_point_t
add_constraints
(
In
PointsBegin
,
In
PointsEnd
,
const
curve_constraints_t
&
constraints
)
{
t_point_t
res
;
point_t
P0
,
P1
,
P2
,
P_n_2
,
P_n_1
,
PN
;
P0
=
*
PointsBegin
;
PN
=
*
(
PointsEnd
-
1
);
P1
=
P0
+
constraints
.
init_vel
/
degree_
;
P_n_1
=
PN
-
constraints
.
end_vel
/
degree_
;
P2
=
constraints
.
init_acc
/
(
degree_
*
(
degree_
-
1
))
+
2
*
P1
-
P0
;
P_n_2
=
constraints
.
end_acc
/
(
degree_
*
(
degree_
-
1
))
+
2
*
P_n_1
-
PN
;
res
.
push_back
(
P0
);
res
.
push_back
(
P1
);
res
.
push_back
(
P2
);
for
(
In
it
=
PointsBegin
+
1
;
it
!=
PointsEnd
-
1
;
++
it
)
res
.
push_back
(
*
it
);
res
.
push_back
(
P_n_2
);
res
.
push_back
(
P_n_1
);
res
.
push_back
(
PN
);
return
res
;
}
/*Operations*/
/*Helpers*/
public:
virtual
time_t
min
()
const
{
return
minBound_
;}
virtual
time_t
max
()
const
{
return
maxBound_
;}
/*Helpers*/
...
...
include/spline/curve_constraint.h
0 → 100644
View file @
e4131820
/**
* \file curve_constraint.h
* \brief struct to define constraints on start / end velocities and acceleration
* on a curve
* \author Steve T.
* \version 0.1
* \date 04/05/2017
*
*/
#ifndef _CLASS_CURVE_CONSTRAINT
#define _CLASS_CURVE_CONSTRAINT
#include "MathDefs.h"
#include <functional>
#include <vector>
namespace
spline
{
template
<
typename
Point
>
struct
curve_constraints
{
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
(){}
point_t
init_vel
;
point_t
init_acc
;
point_t
end_vel
;
point_t
end_acc
;
};
}
#endif //_CLASS_CUBICZEROVELACC
include/spline/spline_deriv_constraint.h
View file @
e4131820
...
...
@@ -21,6 +21,7 @@
#define _CLASS_CUBICZEROVELACC
#include "exact_cubic.h"
#include "curve_constraint.h"
#include "MathDefs.h"
...
...
@@ -53,19 +54,7 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
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
;
public:
struct
spline_constraints
{
spline_constraints
()
:
init_vel
(
point_t
::
Zero
()),
init_acc
(
init_vel
),
end_vel
(
init_vel
),
end_acc
(
init_vel
){}
~
spline_constraints
(){}
point_t
init_vel
;
point_t
init_acc
;
point_t
end_vel
;
point_t
end_acc
;
};
typedef
curve_constraints
<
point_t
>
spline_constraints
;
/* Constructors - destructors */
public:
...
...
python/spline_python.cpp
View file @
e4131820
...
...
@@ -2,6 +2,7 @@
#include "spline/spline_curve.h"
#include "spline/exact_cubic.h"
#include "spline/spline_deriv_constraint.h"
#include "spline/curve_constraint.h"
#include <vector>
...
...
@@ -36,14 +37,15 @@ typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t
typedef
spline
::
spline_deriv_constraint
<
real
,
real
,
3
,
true
,
point_t
,
t_point_t
>
spline_deriv_constraint_t
;
typedef
spline_deriv_constraint_t
::
spline_constraints
spline_constraints_t
;
typedef
spline
::
curve_constraints
<
point_t
>
curve_constraints_t
;
typedef
spline
::
curve_constraints
<
point6_t
>
curve_constraints6_t
;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bezier_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bezier6_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
spline_curve_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
exact_cubic_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
splin
e_constraints_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
curv
e_constraints_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
spline_deriv_constraint_t
)
namespace
spline
...
...
@@ -58,31 +60,56 @@ T_Point vectorFromEigenArray(const PointList& array)
return
res
;
}
bezier_t
*
wrapBezierConstructor
(
const
point_list_t
&
array
)
template
<
typename
Bezier
,
typename
PointList
,
typename
T_Point
>
Bezier
*
wrapBezierConstructorTemplate
(
const
PointList
&
array
,
const
real
lb
=
0.
,
const
real
ub
=
1.
)
{
t_p
oint
_t
asVector
=
vectorFromEigenArray
<
p
oint
_list_t
,
t_p
oint
_t
>
(
array
);
return
new
b
ezier
_t
(
asVector
.
begin
(),
asVector
.
end
());
T_P
oint
asVector
=
vectorFromEigenArray
<
P
oint
List
,
T_P
oint
>
(
array
);
return
new
B
ezier
(
asVector
.
begin
(),
asVector
.
end
()
,
lb
,
ub
);
}
template
<
typename
Bezier
,
typename
PointList
,
typename
T_Point
,
typename
CurveConstraints
>
Bezier
*
wrapBezierConstructorConstraintsTemplate
(
const
PointList
&
array
,
const
CurveConstraints
&
constraints
,
const
real
lb
=
0.
,
const
real
ub
=
1.
)
{
T_Point
asVector
=
vectorFromEigenArray
<
PointList
,
T_Point
>
(
array
);
return
new
Bezier
(
asVector
.
begin
(),
asVector
.
end
(),
constraints
,
lb
,
ub
);
}
/*3D constructors */
bezier_t
*
wrapBezierConstructor
(
const
point_list_t
&
array
)
{
return
wrapBezierConstructorTemplate
<
bezier_t
,
point_list_t
,
t_point_t
>
(
array
)
;
}
bezier_t
*
wrapBezierConstructorBounds
(
const
point_list_t
&
array
,
const
real
lb
,
const
real
ub
)
{
t_point_t
asVector
=
vectorFromEigenArray
<
point_list_t
,
t_point_t
>
(
array
);
return
new
bezier_t
(
asVector
.
begin
(),
asVector
.
end
(),
lb
,
ub
);
return
wrapBezierConstructorTemplate
<
bezier_t
,
point_list_t
,
t_point_t
>
(
array
,
lb
,
ub
)
;
}
bezier_t
*
wrapBezierConstructorConstraints
(
const
point_list_t
&
array
,
const
curve_constraints_t
&
constraints
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier_t
,
point_list_t
,
t_point_t
,
curve_constraints_t
>
(
array
,
constraints
)
;
}
bezier_t
*
wrapBezierConstructorBoundsConstraints
(
const
point_list_t
&
array
,
const
curve_constraints_t
&
constraints
,
const
real
lb
,
const
real
ub
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier_t
,
point_list_t
,
t_point_t
,
curve_constraints_t
>
(
array
,
constraints
,
lb
,
ub
)
;
}
/*END 3D constructors */
/*6D constructors */
bezier6_t
*
wrapBezierConstructor6
(
const
point_list6_t
&
array
)
{
t_point6_t
asVector
=
vectorFromEigenArray
<
point_list6_t
,
t_point6_t
>
(
array
);
return
new
bezier6_t
(
asVector
.
begin
(),
asVector
.
end
());
return
wrapBezierConstructorTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
>
(
array
)
;
}
bezier6_t
*
wrapBezierConstructorBounds6
(
const
point_list6_t
&
array
,
const
real
lb
,
const
real
ub
)
{
t_point6_t
asVector
=
vectorFromEigenArray
<
point_list6_t
,
t_point6_t
>
(
array
);
return
new
bezier6_t
(
asVector
.
begin
(),
asVector
.
end
(),
lb
,
ub
);
return
wrapBezierConstructorTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
>
(
array
,
lb
,
ub
)
;
}
bezier6_t
*
wrapBezierConstructor6Constraints
(
const
point_list6_t
&
array
,
const
curve_constraints6_t
&
constraints
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
,
curve_constraints6_t
>
(
array
,
constraints
)
;
}
bezier6_t
*
wrapBezierConstructorBounds6Constraints
(
const
point_list6_t
&
array
,
const
curve_constraints6_t
&
constraints
,
const
real
lb
,
const
real
ub
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
,
curve_constraints6_t
>
(
array
,
constraints
,
lb
,
ub
)
;
}
/*END 6D constructors */
spline_curve_t
*
wrapSplineConstructor
(
const
coeff_t
&
array
)
{
...
...
@@ -118,7 +145,7 @@ exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoi
}
spline_deriv_constraint_t
*
wrapSplineDerivConstraint
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
,
const
splin
e_constraints_t
&
constraints
)
spline_deriv_constraint_t
*
wrapSplineDerivConstraint
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
,
const
curv
e_constraints_t
&
constraints
)
{
t_waypoint_t
wps
=
getWayPoints
(
array
,
time_wp
);
return
new
spline_deriv_constraint_t
(
wps
.
begin
(),
wps
.
end
(),
constraints
);
...
...
@@ -130,42 +157,42 @@ spline_deriv_constraint_t* wrapSplineDerivConstraintNoConstraints(const coeff_t&
return
new
spline_deriv_constraint_t
(
wps
.
begin
(),
wps
.
end
());
}
point_t
get_init_vel
(
const
splin
e_constraints_t
&
c
)
point_t
get_init_vel
(
const
curv
e_constraints_t
&
c
)
{
return
c
.
init_vel
;
}
point_t
get_init_acc
(
const
splin
e_constraints_t
&
c
)
point_t
get_init_acc
(
const
curv
e_constraints_t
&
c
)
{
return
c
.
init_acc
;
}
point_t
get_end_vel
(
const
splin
e_constraints_t
&
c
)
point_t
get_end_vel
(
const
curv
e_constraints_t
&
c
)
{
return
c
.
end_vel
;
}
point_t
get_end_acc
(
const
splin
e_constraints_t
&
c
)
point_t
get_end_acc
(
const
curv
e_constraints_t
&
c
)
{
return
c
.
end_acc
;
}
void
set_init_vel
(
splin
e_constraints_t
&
c
,
const
point_t
&
val
)
void
set_init_vel
(
curv
e_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
init_vel
=
val
;
}
void
set_init_acc
(
splin
e_constraints_t
&
c
,
const
point_t
&
val
)
void
set_init_acc
(
curv
e_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
init_acc
=
val
;
}
void
set_end_vel
(
splin
e_constraints_t
&
c
,
const
point_t
&
val
)
void
set_end_vel
(
curv
e_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
end_vel
=
val
;
}
void
set_end_acc
(
splin
e_constraints_t
&
c
,
const
point_t
&
val
)
void
set_end_acc
(
curv
e_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
end_acc
=
val
;
}
...
...
@@ -193,6 +220,8 @@ BOOST_PYTHON_MODULE(spline)
(
"bezier6"
,
no_init
)
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructor6
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructorBounds6
))
//.def("__init__", make_constructor(&wrapBezierConstructor6Constraints))
//.def("__init__", make_constructor(&wrapBezierConstructorBounds6Constraints))
.
def
(
"min"
,
&
bezier6_t
::
min
)
.
def
(
"max"
,
&
bezier6_t
::
max
)
.
def
(
"__call__"
,
&
bezier6_t
::
operator
())
...
...
@@ -210,6 +239,8 @@ BOOST_PYTHON_MODULE(spline)
(
"bezier"
,
no_init
)
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructor
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructorBounds
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructorConstraints
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructorBoundsConstraints
))
.
def
(
"min"
,
&
bezier_t
::
min
)
.
def
(
"max"
,
&
bezier_t
::
max
)
.
def
(
"__call__"
,
&
bezier_t
::
operator
())
...
...
@@ -246,15 +277,15 @@ BOOST_PYTHON_MODULE(spline)
/** END bezier curve**/
/** BEGIN
splin
e constraints**/
class_
<
splin
e_constraints_t
>
(
"
splin
e_constraints"
,
init
<>
())
/** BEGIN
curv
e constraints**/
class_
<
curv
e_constraints_t
>
(
"
curv
e_constraints"
,
init
<>
())
.
add_property
(
"init_vel"
,
&
get_init_vel
,
&
set_init_vel
)
.
add_property
(
"init_acc"
,
&
get_init_acc
,
&
set_init_acc
)
.
add_property
(
"end_vel"
,
&
get_end_vel
,
&
set_end_vel
)
.
add_property
(
"end_acc"
,
&
get_end_acc
,
&
set_end_acc
)
;
/** END
splin
e constraints**/
/** END
curv
e constraints**/
/** BEGIN spline_deriv_constraints**/
...
...
python/test/test.py
View file @
e4131820
from
spline
import
bezier
,
spline
,
exact_cubic
,
spline_constraints
,
splin
e_constraints
,
spline_deriv_constraint
from
spline
import
bezier
,
bezier6
,
spline
,
exact_cubic
,
curv
e_constraints
,
spline_deriv_constraint
from
numpy
import
matrix
from
numpy.linalg
import
norm
waypoints
=
matrix
([[
1.
,
2.
,
3.
],[
4.
,
5.
,
6.
]]).
transpose
()
waypoints6
=
matrix
([[
1.
,
2.
,
3.
,
7.
,
5.
,
5.
],[
4.
,
5.
,
6.
,
4.
,
5.
,
6.
]]).
transpose
()
time_waypoints
=
matrix
([
0.
,
1.
])
#testing bezier curve
a
=
bezier6
(
waypoints6
)
a
=
bezier
(
waypoints
,
-
1.
,
3.
)
a
=
bezier
(
waypoints
)
assert
(
a
.
degree
==
a
.
nbWaypoints
-
1
)
a
.
min
()
a
.
max
()
...
...
@@ -31,6 +33,18 @@ for i in range(10):
assert
(
prim
(
0
)
==
matrix
([
0.
,
0.
,
0.
])).
all
()
#testing bezier with constraints
c
=
curve_constraints
();
c
.
init_vel
=
matrix
([
0.
,
1.
,
1.
]);
c
.
end_vel
=
matrix
([
0.
,
1.
,
1.
]);
c
.
init_acc
=
matrix
([
0.
,
1.
,
-
1.
]);
c
.
end_acc
=
matrix
([
0.
,
100.
,
1.
]);
a
=
bezier
(
waypoints
,
c
)
assert
norm
(
a
.
derivate
(
0
,
1
)
-
c
.
init_vel
)
<
1e-10
assert
norm
(
a
.
derivate
(
1
,
2
)
-
c
.
end_acc
)
<
1e-10
#testing spline function
a
=
spline
(
waypoints
)
...
...
@@ -50,7 +64,7 @@ assert((a.derivate(0.4,0) == a(0.4)).all())
a
.
derivate
(
0.4
,
2
)
#testing spline_deriv_constraints
c
=
splin
e_constraints
();
c
=
curv
e_constraints
();
c
.
init_vel
;
c
.
end_vel
;
c
.
init_acc
;
...
...
src/tests/spline_test/Main.cpp
View file @
e4131820
...
...
@@ -60,7 +60,7 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
if
((
pt1
-
pt2
).
norm
()
>
margin
&&
!
notequal
)
{
error
=
true
;
std
::
cout
<<
errmsg
<<
pt1
<<
" ; "
<<
pt2
<<
std
::
endl
;
std
::
cout
<<
errmsg
<<
pt1
.
transpose
()
<<
" ; "
<<
pt2
.
transpose
()
<<
std
::
endl
;
}
}
...
...
@@ -247,6 +247,37 @@ void BezierDerivativeCurveTest(bool& error)
ComparePoints
(
point_t
::
Zero
(),
cf3
.
derivate
(
0.
,
100
),
errMsg
,
error
);
}
void
BezierDerivativeCurveConstraintTest
(
bool
&
error
)
{
std
::
string
errMsg
(
"In test BezierDerivativeCurveConstraintTest ; unexpected result for x "
);
point_t
a
(
1
,
2
,
3
);
point_t
b
(
2
,
3
,
4
);
point_t
c
(
3
,
4
,
5
);
bezier_curve_t
::
curve_constraints_t
constraints
;
constraints
.
init_vel
=
point_t
(
-
1
,
-
1
,
-
1
);
constraints
.
init_acc
=
point_t
(
-
2
,
-
2
,
-
2
);
constraints
.
end_vel
=
point_t
(
-
10
,
-
10
,
-
10
);
constraints
.
end_acc
=
point_t
(
-
20
,
-
20
,
-
20
);
std
::
vector
<
point_t
>
params
;
params
.
push_back
(
a
);
params
.
push_back
(
b
);
params
.
push_back
(
c
);
bezier_curve_t
cf3
(
params
.
begin
(),
params
.
end
(),
constraints
);
assert
(
cf3
.
degree_
==
params
.
size
()
+
3
);
assert
(
cf3
.
size_
==
params
.
size
()
+
4
);
ComparePoints
(
a
,
cf3
(
0
),
errMsg
,
error
);
ComparePoints
(
c
,
cf3
(
1
),
errMsg
,
error
);
ComparePoints
(
constraints
.
init_vel
,
cf3
.
derivate
(
0.
,
1
),
errMsg
,
error
);
ComparePoints
(
constraints
.
end_vel
,
cf3
.
derivate
(
1.
,
1
),
errMsg
,
error
);
ComparePoints
(
constraints
.
init_acc
,
cf3
.
derivate
(
0.
,
2
),
errMsg
,
error
);
ComparePoints
(
constraints
.
end_vel
,
cf3
.
derivate
(
1.
,
1
),
errMsg
,
error
);
ComparePoints
(
constraints
.
end_acc
,
cf3
.
derivate
(
1.
,
2
),
errMsg
,
error
);
}
/*Exact Cubic Function tests*/
...
...
@@ -599,6 +630,7 @@ int main(int /*argc*/, char** /*argv[]*/)
EffectorSplineRotationWayPointRotationTest
(
error
);
BezierCurveTest
(
error
);
BezierDerivativeCurveTest
(
error
);
BezierDerivativeCurveConstraintTest
(
error
);
if
(
error
)
{
std
::
cout
<<
"There were some errors
\n
"
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a 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