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
005fa588
Commit
005fa588
authored
Aug 23, 2019
by
JasonChmn
Committed by
Pierre Fernbach
Sep 03, 2019
Browse files
[dynamic dim - bezier curve] Remove dim from template of bezier curve + binding python
parent
5b1a835e
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/curves/bezier_curve.h
View file @
005fa588
...
...
@@ -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,11 +398,15 @@ 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*/
...
...
@@ -425,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
);
}
...
...
@@ -451,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/linear_variable.h
View file @
005fa588
...
...
@@ -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
;
...
...
python/curves_python.cpp
View file @
005fa588
...
...
@@ -30,6 +30,7 @@ typedef std::pair<point3_t, point3_t> pair_point3_tangent_t;
typedef
Eigen
::
Matrix
<
real
,
3
,
Eigen
::
Dynamic
>
point3_list_t
;
typedef
std
::
vector
<
point3_t
,
Eigen
::
aligned_allocator
<
point3_t
>
>
t_point3_t
;
typedef
std
::
vector
<
pair_point3_tangent_t
,
Eigen
::
aligned_allocator
<
pair_point3_tangent_t
>
>
t_pair_point3_tangent_t
;
typedef
curves
::
curve_constraints
<
point3_t
>
curve_constraints3_t
;
// XD
typedef
Eigen
::
VectorXd
pointX_t
;
typedef
Eigen
::
Matrix
<
double
,
Eigen
::
Dynamic
,
1
,
0
,
Eigen
::
Dynamic
,
1
>
ret_pointX_t
;
...
...
@@ -37,6 +38,7 @@ typedef std::pair<pointX_t, pointX_t> pair_pointX_tangent_t;
typedef
Eigen
::
MatrixXd
pointX_list_t
;
typedef
std
::
vector
<
pointX_t
,
Eigen
::
aligned_allocator
<
point3_t
>
>
t_pointX_t
;
typedef
std
::
vector
<
pair_pointX_tangent_t
,
Eigen
::
aligned_allocator
<
pair_pointX_tangent_t
>
>
t_pair_pointX_tangent_t
;
typedef
curves
::
curve_constraints
<
pointX_t
>
curve_constraints_t
;
// Else
typedef
std
::
pair
<
real
,
point3_t
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
...
...
@@ -44,9 +46,9 @@ typedef std::vector<Waypoint6> T_Waypoint6;
// Dynamic dim
typedef
curves
::
cubic_hermite_spline
<
real
,
real
,
true
,
pointX_t
>
cubic_hermite_spline_t
;
typedef
curves
::
bezier_curve
<
real
,
real
,
true
,
pointX_t
>
bezier_t
;
// 3D
typedef
curves
::
bezier_curve
<
real
,
real
,
3
,
true
,
point3_t
>
bezier3_t
;
typedef
curves
::
polynomial
<
real
,
real
,
3
,
true
,
point3_t
,
t_point3_t
>
polynomial3_t
;
typedef
curves
::
exact_cubic
<
real
,
real
,
3
,
true
,
point3_t
,
t_point3_t
>
exact_cubic3_t
;
typedef
polynomial3_t
::
coeff_t
coeff_t
;
...
...
@@ -54,20 +56,20 @@ typedef std::pair<real, point3_t> waypoint3_t;
typedef
std
::
vector
<
waypoint3_t
,
Eigen
::
aligned_allocator
<
point3_t
>
>
t_waypoint3_t
;
typedef
curves
::
piecewise_curve
<
real
,
real
,
3
,
true
,
point3_t
,
t_point3_t
,
polynomial3_t
>
piecewise_polynomial3_curve_t
;
typedef
curves
::
piecewise_curve
<
real
,
real
,
3
,
true
,
point3_t
,
t_point3_t
,
bezier
3
_t
>
piecewise_bezier3_curve_t
;
typedef
curves
::
piecewise_curve
<
real
,
real
,
3
,
true
,
point3_t
,
t_point3_t
,
bezier_t
>
piecewise_bezier3_curve_t
;
typedef
curves
::
piecewise_curve
<
real
,
real
,
3
,
true
,
point3_t
,
t_point3_t
,
cubic_hermite_spline_t
>
piecewise_cubic_hermite_curve_t
;
typedef
curves
::
Bern
<
double
>
bernstein_t
;
typedef
curves
::
curve_constraints
<
point3_t
>
curve_constraints3_t
;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bernstein_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bezier3_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
cubic_hermite_spline_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bezier_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
curve_constraints_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
polynomial3_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
exact_cubic3_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
cubic_hermite_spline_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
curve_constraints3_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
piecewise_polynomial3_curve_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
piecewise_bezier3_curve_t
)
...
...
@@ -95,23 +97,23 @@ namespace curves
/* End Template constructor bezier */
/*3D constructors bezier */
bezier
3
_t
*
wrapBezierConstructor
3
(
const
point
3
_list_t
&
array
)
bezier_t
*
wrapBezierConstructor
(
const
point
X
_list_t
&
array
)
{
return
wrapBezierConstructorTemplate
<
bezier
3
_t
,
point
3
_list_t
,
t_point
3
_t
>
(
array
)
;
return
wrapBezierConstructorTemplate
<
bezier_t
,
point
X
_list_t
,
t_point
X
_t
>
(
array
)
;
}
bezier
3
_t
*
wrapBezierConstructorBounds
3
(
const
point
3
_list_t
&
array
,
const
real
T_min
,
const
real
T_max
)
bezier_t
*
wrapBezierConstructorBounds
(
const
point
X
_list_t
&
array
,
const
real
T_min
,
const
real
T_max
)
{
return
wrapBezierConstructorTemplate
<
bezier
3
_t
,
point
3
_list_t
,
t_point
3
_t
>
(
array
,
T_min
,
T_max
)
;
return
wrapBezierConstructorTemplate
<
bezier_t
,
point
X
_list_t
,
t_point
X
_t
>
(
array
,
T_min
,
T_max
)
;
}
bezier
3
_t
*
wrapBezierConstructor
3
Constraints
(
const
point
3
_list_t
&
array
,
const
curve_constraints
3
_t
&
constraints
)
bezier_t
*
wrapBezierConstructorConstraints
(
const
point
X
_list_t
&
array
,
const
curve_constraints_t
&
constraints
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier
3
_t
,
point
3
_list_t
,
t_point
3
_t
,
curve_constraints
3
_t
>
(
array
,
constraints
)
;
return
wrapBezierConstructorConstraintsTemplate
<
bezier_t
,
point
X
_list_t
,
t_point
X
_t
,
curve_constraints_t
>
(
array
,
constraints
)
;
}
bezier
3
_t
*
wrapBezierConstructorBounds
3
Constraints
(
const
point
3
_list_t
&
array
,
const
curve_constraints
3
_t
&
constraints
,
const
real
T_min
,
const
real
T_max
)
bezier_t
*
wrapBezierConstructorBoundsConstraints
(
const
point
X
_list_t
&
array
,
const
curve_constraints_t
&
constraints
,
const
real
T_min
,
const
real
T_max
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier
3
_t
,
point
3
_list_t
,
t_point
3
_t
,
curve_constraints
3
_t
>
(
array
,
constraints
,
T_min
,
T_max
)
;
return
wrapBezierConstructorConstraintsTemplate
<
bezier_t
,
point
X
_list_t
,
t_point
X
_t
,
curve_constraints_t
>
(
array
,
constraints
,
T_min
,
T_max
)
;
}
/*END 3D constructors bezier */
...
...
@@ -163,7 +165,7 @@ namespace curves
{
return
new
piecewise_polynomial3_curve_t
();
}
piecewise_bezier3_curve_t
*
wrapPiecewiseBezier3CurveConstructor
(
const
bezier
3
_t
&
bc
)
piecewise_bezier3_curve_t
*
wrapPiecewiseBezier3CurveConstructor
(
const
bezier_t
&
bc
)
{
return
new
piecewise_bezier3_curve_t
(
bc
);
}
...
...
@@ -192,21 +194,6 @@ namespace curves
return
res
;
}
template
<
typename
BezierType
,
int
dim
>
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
wayPointsToList
(
const
BezierType
&
self
)
{
typedef
typename
BezierType
::
t_point_t
t_point
;
typedef
typename
BezierType
::
t_point_t
::
const_iterator
cit_point
;
const
t_point
&
wps
=
self
.
waypoints
();
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
res
(
dim
,
wps
.
size
());
int
col
=
0
;
for
(
cit_point
cit
=
wps
.
begin
();
cit
!=
wps
.
end
();
++
cit
,
++
col
)
{
res
.
block
<
dim
,
1
>
(
0
,
col
)
=
*
cit
;
}
return
res
;
}
exact_cubic3_t
*
wrapExactCubic3Constructor
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
)
{
t_waypoint3_t
wps
=
getWayPoints
(
array
,
time_wp
);
...
...
@@ -220,6 +207,7 @@ namespace curves
return
new
exact_cubic3_t
(
wps
.
begin
(),
wps
.
end
(),
constraints
);
}
/// For constraints 3D
point3_t
get_init_vel
(
const
curve_constraints3_t
&
c
)
{
return
c
.
init_vel
;
...
...
@@ -259,6 +247,46 @@ namespace curves
{
c
.
end_acc
=
val
;
}
/// For constraints XD
point3_t
get_init_velX
(
const
curve_constraints_t
&
c
)
{
return
c
.
init_vel
;
}
point3_t
get_init_accX
(
const
curve_constraints_t
&
c
)
{
return
c
.
init_acc
;
}
point3_t
get_end_velX
(
const
curve_constraints_t
&
c
)
{
return
c
.
end_vel
;
}
point3_t
get_end_accX
(
const
curve_constraints_t
&
c
)
{
return
c
.
end_acc
;
}
void
set_init_velX
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
init_vel
=
val
;
}
void
set_init_accX
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
init_acc
=
val
;
}
void
set_end_velX
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
end_vel
=
val
;
}
void
set_end_accX
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
end_acc
=
val
;
}
/* End wrap exact cubic spline */
...
...
@@ -277,28 +305,27 @@ namespace curves
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
/** BEGIN bezier curve**/
class_
<
bezier3_t
>
(
"bezier3"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructor3
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructorBounds3
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructor3Constraints
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructorBounds3Constraints
))
.
def
(
"min"
,
&
bezier3_t
::
min
)
.
def
(
"max"
,
&
bezier3_t
::
max
)
.
def
(
"dim"
,
&
bezier3_t
::
dim
)
.
def
(
"__call__"
,
&
bezier3_t
::
operator
())
.
def
(
"derivate"
,
&
bezier3_t
::
derivate
)
.
def
(
"compute_derivate"
,
&
bezier3_t
::
compute_derivate
)
.
def
(
"compute_primitive"
,
&
bezier3_t
::
compute_primitive
)
.
def
(
"waypoints"
,
&
wayPointsToList
<
bezier3_t
,
3
>
)
.
def
(
"saveAsText"
,
&
bezier3_t
::
saveAsText
<
bezier3_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a text file."
)
.
def
(
"loadFromText"
,
&
bezier3_t
::
loadFromText
<
bezier3_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a text file."
)
.
def
(
"saveAsXML"
,
&
bezier3_t
::
saveAsXML
<
bezier3_t
>
,
bp
::
args
(
"filename"
,
"tag_name"
),
"Saves *this inside a XML file."
)
.
def
(
"loadFromXML"
,
&
bezier3_t
::
loadFromXML
<
bezier3_t
>
,
bp
::
args
(
"filename"
,
"tag_name"
),
"Loads *this from a XML file."
)
.
def
(
"saveAsBinary"
,
&
bezier3_t
::
saveAsBinary
<
bezier3_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a binary file."
)
.
def
(
"loadFromBinary"
,
&
bezier3_t
::
loadFromBinary
<
bezier3_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a binary file."
)
.
def_readonly
(
"degree"
,
&
bezier3_t
::
degree_
)
.
def_readonly
(
"nbWaypoints"
,
&
bezier3_t
::
size_
)
//.def(SerializableVisitor<bezier3_t>())
class_
<
bezier_t
>
(
"bezier"
,
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
(
"dim"
,
&
bezier_t
::
dim
)
.
def
(
"__call__"
,
&
bezier_t
::
operator
())
.
def
(
"derivate"
,
&
bezier_t
::
derivate
)
.
def
(
"compute_derivate"
,
&
bezier_t
::
compute_derivate
)
.
def
(
"compute_primitive"
,
&
bezier_t
::
compute_primitive
)
.
def
(
"saveAsText"
,
&
bezier_t
::
saveAsText
<
bezier_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a text file."
)
.
def
(
"loadFromText"
,
&
bezier_t
::
loadFromText
<
bezier_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a text file."
)
.
def
(
"saveAsXML"
,
&
bezier_t
::
saveAsXML
<
bezier_t
>
,
bp
::
args
(
"filename"
,
"tag_name"
),
"Saves *this inside a XML file."
)
.
def
(
"loadFromXML"
,
&
bezier_t
::
loadFromXML
<
bezier_t
>
,
bp
::
args
(
"filename"
,
"tag_name"
),
"Loads *this from a XML file."
)
.
def
(
"saveAsBinary"
,
&
bezier_t
::
saveAsBinary
<
bezier_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a binary file."
)
.
def
(
"loadFromBinary"
,
&
bezier_t
::
loadFromBinary
<
bezier_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a binary file."
)
.
def_readonly
(
"degree"
,
&
bezier_t
::
degree_
)
.
def_readonly
(
"nbWaypoints"
,
&
bezier_t
::
size_
)
//.def(SerializableVisitor<bezier_t>())
;
/** END bezier curve**/
/** BEGIN variable points bezier curve**/
...
...
@@ -461,6 +488,15 @@ namespace curves
.
add_property
(
"end_acc"
,
&
get_end_acc
,
&
set_end_acc
)
;
/** END curve constraints**/
/** BEGIN curve constraints**/
class_
<
curve_constraints_t
>
(
"curve_constraints"
,
init
<>
())
.
add_property
(
"init_vel"
,
&
get_init_velX
,
&
set_init_velX
)
.
add_property
(
"init_acc"
,
&
get_init_accX
,
&
set_init_accX
)
.
add_property
(
"end_vel"
,
&
get_end_velX
,
&
set_end_velX
)
.
add_property
(
"end_acc"
,
&
get_end_accX
,
&
set_end_accX
)
;
/** END curve constraints**/
/** BEGIN bernstein polynomial**/
class_
<
bernstein_t
>
(
"bernstein"
,
init
<
const
unsigned
int
,
const
unsigned
int
>
())
...
...
@@ -468,11 +504,11 @@ namespace curves
;
/** END bernstein polynomial**/
/** BEGIN curves conversion**/
def
(
"polynomial_from_bezier"
,
polynomial_from_curve
<
polynomial3_t
,
bezier
3
_t
>
);
def
(
"polynomial_from_bezier"
,
polynomial_from_curve
<
polynomial3_t
,
bezier_t
>
);
def
(
"polynomial_from_hermite"
,
polynomial_from_curve
<
polynomial3_t
,
cubic_hermite_spline_t
>
);
def
(
"bezier_from_hermite"
,
bezier_from_curve
<
bezier
3
_t
,
cubic_hermite_spline_t
>
);
def
(
"bezier_from_polynomial"
,
bezier_from_curve
<
bezier
3
_t
,
polynomial3_t
>
);
def
(
"hermite_from_bezier"
,
hermite_from_curve
<
cubic_hermite_spline_t
,
bezier
3
_t
>
);
def
(
"bezier_from_hermite"
,
bezier_from_curve
<
bezier_t
,
cubic_hermite_spline_t
>
);
def
(
"bezier_from_polynomial"
,
bezier_from_curve
<
bezier_t
,
polynomial3_t
>
);
def
(
"hermite_from_bezier"
,
hermite_from_curve
<
cubic_hermite_spline_t
,
bezier_t
>
);
def
(
"hermite_from_polynomial"
,
hermite_from_curve
<
cubic_hermite_spline_t
,
polynomial3_t
>
);
/** END curves conversion**/
}
// End BOOST_PYTHON_MODULE
...
...
python/python_variables.h
View file @
005fa588
...
...
@@ -14,7 +14,7 @@ namespace curves
static
const
int
dim
=
3
;
typedef
curves
::
linear_variable
<
dim
,
real
>
linear_variable_3_t
;
typedef
curves
::
variables
<
linear_variable_3_t
>
variables_3_t
;
typedef
curves
::
bezier_curve
<
real
,
real
,
dim
,
true
,
variables_3_t
>
bezier_linear_variable_t
;
typedef
curves
::
bezier_curve
<
real
,
real
,
true
,
variables_3_t
>
bezier_linear_variable_t
;
/*linear variable control points*/
bezier_linear_variable_t
*
wrapBezierLinearConstructor
(
const
point_list_t
&
matrices
,
const
point_list_t
&
vectors
);
...
...
python/test/test.py
View file @
005fa588
...
...
@@ -8,7 +8,7 @@ from numpy.linalg import norm
from
curves
import
(
bezier_from_hermite
,
bezier_from_polynomial
,
hermite_from_polynomial
,
hermite_from_bezier
,
polynomial_from_hermite
,
polynomial_from_bezier
,
cubic_hermite_spline3
,
curve_constraints3
,
exact_cubic3
,
bezier
3
,
cubic_hermite_spline3
,
curve_constraints3
,
curve_constraints
,
exact_cubic3
,
bezier
,
piecewise_bezier3_curve
,
piecewise_cubic_hermite3_curve
,
piecewise_polynomial3_curve
,
polynomial3
)
...
...
@@ -26,15 +26,15 @@ class TestCurves(unittest.TestCase):
# - Variables : degree, nbWayPoints
__EPS
=
1e-6
waypoints
=
matrix
([[
1.
,
2.
,
3.
]]).
T
a
=
bezier
3
(
waypoints
,
0.
,
2.
)
a
=
bezier
(
waypoints
,
0.
,
2.
)
t
=
0.
while
t
<
2.
:
self
.
assertTrue
(
norm
(
a
(
t
)
-
matrix
([
1.
,
2.
,
3.
]).
T
)
<
__EPS
)
t
+=
0.1
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
time_waypoints
=
matrix
([
0.
,
1.
]).
transpose
()
# Create bezier6 and bezier
3
a
=
bezier
3
(
waypoints
,
0.
,
3.
)
# Create bezier6 and bezier
a
=
bezier
(
waypoints
,
0.
,
3.
)
# Test : Degree, min, max, derivate
#self.print_str(("test 1")
self
.
assertEqual
(
a
.
degree
,
a
.
nbWaypoints
-
1
)
...
...
@@ -56,13 +56,13 @@ class TestCurves(unittest.TestCase):
t
=
float
(
i
)
/
10.
self
.
assertTrue
((
a
(
t
)
==
prim
.
derivate
(
t
,
2
)).
all
())
self
.
assertTrue
((
prim
(
0
)
==
matrix
([
0.
,
0.
,
0.
])).
all
())
# Create new bezier
3
curve
# Create new bezier curve
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
],
[
4.
,
5.
,
6.
],
[
4.
,
5.
,
6.
],
[
4.
,
5.
,
6.
]]).
transpose
()
a0
=
bezier
3
(
waypoints
)
a1
=
bezier
3
(
waypoints
,
0.
,
3.
)
a0
=
bezier
(
waypoints
)
a1
=
bezier
(
waypoints
,
0.
,
3.
)
prim0
=
a0
.
compute_primitive
(
1
)
prim1
=
a1
.
compute_primitive
(
1
)
# Check change in argument time_t of bezier
3
# Check change in argument time_t of bezier
for
i
in
range
(
10
):
t
=
float
(
i
)
/
10.
self
.
assertTrue
(
norm
(
a0
(
t
)
-
a1
(
3
*
t
))
<
__EPS
)
...
...
@@ -71,21 +71,32 @@ class TestCurves(unittest.TestCase):
self
.
assertTrue
(
norm
(
prim0
(
t
)
-
prim1
(
t
*
3
)
/
3.
)
<
__EPS
)
self
.
assertTrue
((
prim
(
0
)
==
matrix
([
0.
,
0.
,
0.
])).
all
())
# testing bezier with constraints
c
=
curve_constraints
3
()
c
=
curve_constraints
()
c
.
init_vel
=
matrix
([
0.
,
1.
,
1.
]).
transpose
()
c
.
end_vel
=
matrix
([
0.
,
1.
,
1.
]).
transpose
()
c
.
init_acc
=
matrix
([
0.
,
1.
,
-
1.
]).
transpose
()
c
.
end_acc
=
matrix
([
0.
,
100.
,
1.
]).
transpose
()
#Check derivate with constraints
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
a
=
bezier
3
(
waypoints
,
c
)
a
=
bezier
(
waypoints
,
c
)
self
.
assertTrue
(
norm
(
a
.
derivate
(
0
,
1
)
-
c
.
init_vel
)
<
1e-10
)
self
.
assertTrue
(
norm
(
a
.
derivate
(
1
,
2
)
-
c
.
end_acc
)
<
1e-10
)
# Test serialization : bezier 3
a
.
saveAsText
(
"serialization_curve.test"
)
#waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
b
=
bezier3
()
b
=
bezier
()
b
.
loadFromText
(
"serialization_curve.test"
)
self
.
assertTrue
((
a
(
0.4
)
==
b
(
0.4
)).
all
())
os
.
remove
(
"serialization_curve.test"
)
# Bezier dim 4
waypoints
=
matrix
([[
1.
,
2.
,
3.
,
4.
]]).
T
a
=
bezier
(
waypoints
,
0.
,
2.
)
# Test serialization : bezier of dim 4
a
.
saveAsText
(
"serialization_curve.test"
)
#waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
b
=
bezier
()
b
.
loadFromText
(
"serialization_curve.test"
)
self
.
assertTrue
((
a
(
0.4
)
==
b
(
0.4
)).
all
())
os
.
remove
(
"serialization_curve.test"
)
...
...
@@ -173,12 +184,12 @@ class TestCurves(unittest.TestCase):
os
.
remove
(
"serialization_pc.test"
)
return
def
test_piecewise_bezier
3
_curve
(
self
):
def
test_piecewise_bezier_curve
(
self
):
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
a
=
bezier
3
(
waypoints
,
0.
,
1.
)
b
=
bezier
3
(
waypoints
,
1.
,
2.
)
a
=
bezier
(
waypoints
,
0.
,
1.
)
b
=
bezier
(
waypoints
,
1.
,
2.
)
pc
=
piecewise_bezier3_curve
(
a
)
pc
.
add_curve
(
b
)
pc
.
min
()
...
...
@@ -264,7 +275,7 @@ class TestCurves(unittest.TestCase):
def
test_conversion_curves
(
self
):
__EPS
=
1e-6
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
a
=
bezier
3
(
waypoints
)
a
=
bezier
(
waypoints
)
# converting bezier to polynomial
a_pol
=
polynomial_from_bezier
(
a
)
self
.
assertTrue
(
norm
(
a
(
0.3
)
-
a_pol
(
0.3
))
<
__EPS
)
...
...
tests/Main.cpp
View file @
005fa588
...
...
@@ -23,21 +23,21 @@ namespace curves
typedef
curve_abc
<
double
,
double
,
true
,
point_t
>
curve_abc_t
;
typedef
polynomial
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
>
polynomial_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
bezier_curve
<
double
,
double
,
true
,
point_t
>
bezier_curve_t
;
typedef
cubic_hermite_spline
<
double
,
double
,
true
,
point_t
>
cubic_hermite_spline_t
;
typedef
exact_cubic
<
double
,
double
,
1
,
true
,
Eigen
::
Matrix
<
double
,
1
,
1
>
>
exact_cubic_one
;
typedef
piecewise_curve
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
,
polynomial_t
>
piecewise_polynomial_curve_t
;
typedef
piecewise_curve
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
,
bezier_curve_t
>
piecewise_bezier_curve_t
;
typedef
piecewise_curve
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
,
cubic_hermite_spline_t
>
piecewise_cubic_hermite_curve_t
;
typedef
exact_cubic_t
::
spline_constraints
spline_constraints_t
;
typedef
std
::
pair
<
double
,
point_t
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
typedef
Eigen
::
Matrix
<
double
,
1
,
1
>
point_one
;
typedef
polynomial
<
double
,
double
,
1
,
true
,
point_one
>
polynom_one
;
typedef
exact_cubic
<
double
,
double
,
1
,
true
,
point_one
>
exact_cubic_one
;
typedef
std
::
pair
<
double
,
point_one
>
WaypointOne
;
typedef
std
::
vector
<
WaypointOne
>
T_WaypointOne
;
typedef
cubic_hermite_spline
<
double
,
double
,
true
,
point_t
>
cubic_hermite_spline_t
;
typedef
std
::
pair
<
point_t
,
tangent_t
>
pair_point_tangent_t
;
typedef
std
::
vector
<
pair_point_tangent_t
,
Eigen
::
aligned_allocator
<
pair_point_tangent_t
>
>
t_pair_point_tangent_t
;
typedef
piecewise_curve
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
,
polynomial_t
>
piecewise_polynomial_curve_t
;
typedef
piecewise_curve
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
,
bezier_curve_t
>
piecewise_bezier_curve_t
;
typedef
piecewise_curve
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
,
cubic_hermite_spline_t
>
piecewise_cubic_hermite_curve_t
;
const
double
margin
=
1e-3
;
bool
QuasiEqual
(
const
double
a
,
const
double
b
)
{
...
...
@@ -1245,7 +1245,7 @@ void curveAbcDimDynamicTest(bool& error)
typedef
polynomial
<
double
,
double
,
3
,
true
>
polynomial_test_t
;
typedef
exact_cubic
<
double
,
double
,
3
,
true
>
exact_cubic_test_t
;
typedef
exact_cubic_test_t
::
spline_constraints
spline_constraints_test_t
;
typedef
bezier_curve
<
double
,
double
,
3
,
true
>
bezier_curve_test_t
;
typedef
bezier_curve
<
double
,
double
,
true
>
bezier_curve_test_t
;
typedef
cubic_hermite_spline
<
double
,
double
,
true
>
cubic_hermite_spline_test_t
;
curve_abc_test_t
*
pt_curve_abc
;
// POLYNOMIAL
...
...
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