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
5e0243f7
Commit
5e0243f7
authored
Aug 23, 2019
by
JasonChmn
Committed by
Pierre Fernbach
Sep 03, 2019
Browse files
[dynamic dim - exact cubic/constraints/helpers] Remove dim from template + binding python
parent
88ff7cce
Changes
7
Hide whitespace changes
Inline
Side-by-side
include/curves/curve_constraint.h
View file @
5e0243f7
...
...
@@ -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 @
5e0243f7
...
...
@@ -37,7 +37,7 @@ 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
,
Safe
,
Point
,
T_Point
>
>
...
...
@@ -53,9 +53,9 @@ 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
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 */
...
...
@@ -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.
...
...
@@ -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
;
...
...
include/curves/helpers/effector_spline.h
View file @
5e0243f7
...
...
@@ -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 @
5e0243f7
...
...
@@ -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
;
...
...
@@ -102,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.
...
...
python/curves_python.cpp
View file @
5e0243f7
...
...
@@ -23,33 +23,18 @@
using
namespace
curves
;
typedef
double
real
;
typedef
Eigen
::
VectorXd
time_waypoints_t
;
// 3D
typedef
Eigen
::
Vector3d
point3_t
;
typedef
Eigen
::
Matrix
<
double
,
3
,
1
,
0
,
3
,
1
>
ret_point3_t
;
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
;
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
<
point
3
_t
>
>
t_pointX_t
;
typedef
std
::
vector
<
pointX_t
,
Eigen
::
aligned_allocator
<
point
X
_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
;
typedef
std
::
vector
<
Waypoint6
>
T_Waypoint6
;
// 3D
typedef
curves
::
exact_cubic
<
real
,
real
,
3
,
true
,
point3_t
,
t_point3_t
>
exact_cubic3_t
;
typedef
std
::
pair
<
real
,
point3_t
>
waypoint3_t
;
typedef
std
::
vector
<
waypoint3_t
,
Eigen
::
aligned_allocator
<
point3_t
>
>
t_waypoint3_t
;
typedef
std
::
pair
<
real
,
pointX_t
>
waypoint_t
;
typedef
std
::
vector
<
waypoint_t
>
t_waypoint_t
;
//
Dynamic dim
//
Curves
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
;
typedef
curves
::
polynomial
<
real
,
real
,
true
,
pointX_t
,
t_pointX_t
>
polynomial_t
;
...
...
@@ -57,12 +42,12 @@ typedef polynomial_t::coeff_t coeff_t;
typedef
curves
::
piecewise_curve
<
real
,
real
,
true
,
pointX_t
,
t_pointX_t
,
polynomial_t
>
piecewise_polynomial_curve_t
;
typedef
curves
::
piecewise_curve
<
real
,
real
,
true
,
pointX_t
,
t_pointX_t
,
bezier_t
>
piecewise_bezier_curve_t
;
typedef
curves
::
piecewise_curve
<
real
,
real
,
true
,
pointX_t
,
t_pointX_t
,
cubic_hermite_spline_t
>
piecewise_cubic_hermite_curve_t
;
typedef
curves
::
exact_cubic
<
real
,
real
,
true
,
pointX_t
,
t_pointX_t
>
exact_cubic_t
;
typedef
curves
::
Bern
<
double
>
bernstein_t
;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bernstein_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
cubic_hermite_spline_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bezier_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
polynomial_t
)
...
...
@@ -70,9 +55,7 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
piecewise_polynomial_curve_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
piecewise_bezier_curve_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
piecewise_cubic_hermite_curve_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
exact_cubic3_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
curve_constraints3_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
exact_cubic_t
)
namespace
curves
{
...
...
@@ -183,9 +166,9 @@ namespace curves
/* end wrap piecewise polynomial curve */
/* Wrap exact cubic spline */
t_waypoint
3
_t
getWayPoints
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
)
t_waypoint_t
getWayPoints
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
)
{
t_waypoint
3
_t
res
;
t_waypoint_t
res
;
for
(
int
i
=
0
;
i
<
array
.
cols
();
++
i
)
{
res
.
push_back
(
std
::
make_pair
(
time_wp
(
i
),
array
.
col
(
i
)));
...
...
@@ -193,96 +176,56 @@ namespace curves
return
res
;
}
exact_cubic3_t
*
wrapExactCubic3Constructor
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
)
{
t_waypoint3_t
wps
=
getWayPoints
(
array
,
time_wp
);
return
new
exact_cubic3_t
(
wps
.
begin
(),
wps
.
end
());
}
exact_cubic3_t
*
wrapExactCubic3ConstructorConstraint
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
,
const
curve_constraints3_t
&
constraints
)
{
t_waypoint3_t
wps
=
getWayPoints
(
array
,
time_wp
);
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
;
}
point3_t
get_init_acc
(
const
curve_constraints3_t
&
c
)
{
return
c
.
init_acc
;
}
point3_t
get_end_vel
(
const
curve_constraints3_t
&
c
)
{
return
c
.
end_vel
;
}
point3_t
get_end_acc
(
const
curve_constraints3_t
&
c
)
{
return
c
.
end_acc
;
}
void
set_init_vel
(
curve_constraints3_t
&
c
,
const
point3_t
&
val
)
exact_cubic_t
*
wrapExactCubicConstructor
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
)
{
c
.
init_vel
=
val
;
}
void
set_init_acc
(
curve_constraints3_t
&
c
,
const
point3_t
&
val
)
{
c
.
init_acc
=
val
;
t_waypoint_t
wps
=
getWayPoints
(
array
,
time_wp
);
return
new
exact_cubic_t
(
wps
.
begin
(),
wps
.
end
());
}
void
set_end_vel
(
curve_constraints3_t
&
c
,
const
point3_t
&
val
)
exact_cubic_t
*
wrapExactCubicConstructorConstraint
(
const
coeff_t
&
array
,
const
time_waypoints_t
&
time_wp
,
const
curve_constraints_t
&
constraints
)
{
c
.
end_vel
=
val
;
t_waypoint_t
wps
=
getWayPoints
(
array
,
time_wp
);
return
new
exact_cubic_t
(
wps
.
begin
(),
wps
.
end
(),
constraints
);
}
void
set_end_acc
(
curve_constraints3_t
&
c
,
const
point3_t
&
val
)
{
c
.
end_acc
=
val
;
}
/// For constraints XD
point
3
_t
get_init_vel
X
(
const
curve_constraints_t
&
c
)
point_t
get_init_vel
(
const
curve_constraints_t
&
c
)
{
return
c
.
init_vel
;
}
point
3
_t
get_init_acc
X
(
const
curve_constraints_t
&
c
)
point_t
get_init_acc
(
const
curve_constraints_t
&
c
)
{
return
c
.
init_acc
;
}
point
3
_t
get_end_vel
X
(
const
curve_constraints_t
&
c
)
point_t
get_end_vel
(
const
curve_constraints_t
&
c
)
{
return
c
.
end_vel
;
}
point
3
_t
get_end_acc
X
(
const
curve_constraints_t
&
c
)
point_t
get_end_acc
(
const
curve_constraints_t
&
c
)
{
return
c
.
end_acc
;
}
void
set_init_vel
X
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
void
set_init_vel
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
init_vel
=
val
;
}
void
set_init_acc
X
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
void
set_init_acc
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
init_acc
=
val
;
}
void
set_end_vel
X
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
void
set_end_vel
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
end_vel
=
val
;
}
void
set_end_acc
X
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
void
set_end_acc
(
curve_constraints_t
&
c
,
const
point_t
&
val
)
{
c
.
end_acc
=
val
;
}
...
...
@@ -297,8 +240,6 @@ namespace curves
eigenpy
::
enableEigenPy
();
eigenpy
::
enableEigenPySpecific
<
pointX_t
,
pointX_t
>
();
eigenpy
::
enableEigenPySpecific
<
pointX_list_t
,
pointX_list_t
>
();
eigenpy
::
enableEigenPySpecific
<
point3_t
,
point3_t
>
();
eigenpy
::
enableEigenPySpecific
<
point3_list_t
,
point3_list_t
>
();
eigenpy
::
enableEigenPySpecific
<
coeff_t
,
coeff_t
>
();
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
...
...
@@ -440,24 +381,24 @@ namespace curves
;
/** END piecewise curve function **/
/** BEGIN exact_cubic curve**/
class_
<
exact_cubic
3
_t
>
(
"exact_cubic
3
"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapExactCubic
3
Constructor
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapExactCubic
3
ConstructorConstraint
))
.
def
(
"min"
,
&
exact_cubic
3
_t
::
min
)
.
def
(
"max"
,
&
exact_cubic
3
_t
::
max
)
.
def
(
"dim"
,
&
exact_cubic
3
_t
::
dim
)
.
def
(
"__call__"
,
&
exact_cubic
3
_t
::
operator
())
.
def
(
"derivate"
,
&
exact_cubic
3
_t
::
derivate
)
.
def
(
"getNumberSplines"
,
&
exact_cubic
3
_t
::
getNumberSplines
)
.
def
(
"getSplineAt"
,
&
exact_cubic
3
_t
::
getSplineAt
)
.
def
(
"saveAsText"
,
&
exact_cubic
3
_t
::
saveAsText
<
exact_cubic
3
_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a text file."
)
.
def
(
"loadFromText"
,
&
exact_cubic
3
_t
::
loadFromText
<
exact_cubic
3
_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a text file."
)
.
def
(
"saveAsXML"
,
&
exact_cubic
3
_t
::
saveAsXML
<
exact_cubic
3
_t
>
,
bp
::
args
(
"filename"
,
"tag_name"
),
"Saves *this inside a XML file."
)
.
def
(
"loadFromXML"
,
&
exact_cubic
3
_t
::
loadFromXML
<
exact_cubic
3
_t
>
,
bp
::
args
(
"filename"
,
"tag_name"
),
"Loads *this from a XML file."
)
.
def
(
"saveAsBinary"
,
&
exact_cubic
3
_t
::
saveAsBinary
<
exact_cubic
3
_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a binary file."
)
.
def
(
"loadFromBinary"
,
&
exact_cubic
3
_t
::
loadFromBinary
<
exact_cubic
3
_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a binary file."
)
//.def(SerializableVisitor<exact_cubic
3
_t>())
class_
<
exact_cubic_t
>
(
"exact_cubic"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapExactCubicConstructor
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapExactCubicConstructorConstraint
))
.
def
(
"min"
,
&
exact_cubic_t
::
min
)
.
def
(
"max"
,
&
exact_cubic_t
::
max
)
.
def
(
"dim"
,
&
exact_cubic_t
::
dim
)
.
def
(
"__call__"
,
&
exact_cubic_t
::
operator
())
.
def
(
"derivate"
,
&
exact_cubic_t
::
derivate
)
.
def
(
"getNumberSplines"
,
&
exact_cubic_t
::
getNumberSplines
)
.
def
(
"getSplineAt"
,
&
exact_cubic_t
::
getSplineAt
)
.
def
(
"saveAsText"
,
&
exact_cubic_t
::
saveAsText
<
exact_cubic_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a text file."
)
.
def
(
"loadFromText"
,
&
exact_cubic_t
::
loadFromText
<
exact_cubic_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a text file."
)
.
def
(
"saveAsXML"
,
&
exact_cubic_t
::
saveAsXML
<
exact_cubic_t
>
,
bp
::
args
(
"filename"
,
"tag_name"
),
"Saves *this inside a XML file."
)
.
def
(
"loadFromXML"
,
&
exact_cubic_t
::
loadFromXML
<
exact_cubic_t
>
,
bp
::
args
(
"filename"
,
"tag_name"
),
"Loads *this from a XML file."
)
.
def
(
"saveAsBinary"
,
&
exact_cubic_t
::
saveAsBinary
<
exact_cubic_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a binary file."
)
.
def
(
"loadFromBinary"
,
&
exact_cubic_t
::
loadFromBinary
<
exact_cubic_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a binary file."
)
//.def(SerializableVisitor<exact_cubic_t>())
;
/** END exact_cubic curve**/
/** BEGIN cubic_hermite_spline **/
...
...
@@ -479,23 +420,14 @@ namespace curves
;
/** END cubic_hermite_spline **/
/** BEGIN curve constraints**/
class_
<
curve_constraints
3
_t
>
(
"curve_constraints
3
"
,
init
<>
())
class_
<
curve_constraints_t
>
(
"curve_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 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
>
())
...
...
python/test/test.py
View file @
5e0243f7
...
...
@@ -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_spline
,
curve_constraints3
,
curve_constraints
,
exact_cubic
3
,
bezier
,
cubic_hermite_spline
,
curve_constraints
,
exact_cubic
,
bezier
,
piecewise_bezier_curve
,
piecewise_cubic_hermite_curve
,
piecewise_polynomial_curve
,
polynomial
)
...
...
@@ -21,6 +21,7 @@ class TestCurves(unittest.TestCase):
# return
def
test_bezier
(
self
):
print
(
"test_bezier"
)
# To test :
# - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
# - Variables : degree, nbWayPoints
...
...
@@ -103,6 +104,7 @@ class TestCurves(unittest.TestCase):
return
def
test_polynomial
(
self
):
print
(
"test_polynomial"
)
# To test :
# - Functions : constructor, min, max, derivate, serialize, deserialize
waypoints_0
=
matrix
([[
0.
,
0.
,
0.
],
[
0.
,
0.
,
0.
]]).
transpose
()
...
...
@@ -125,6 +127,7 @@ class TestCurves(unittest.TestCase):
return
def
test_cubic_hermite_spline
(
self
):
print
(
"test_cubic_hermite_spline"
)
points
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
tangents
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
time_points
=
matrix
([
0.
,
1.
]).
transpose
()
...
...
@@ -159,6 +162,7 @@ class TestCurves(unittest.TestCase):
return
def
test_piecewise_polynomial_curve
(
self
):
print
(
"test_piecewise_polynomial_curve"
)
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous, serialize, deserialize
waypoints0
=
matrix
([[
0.
,
0.
,
0.
]]).
transpose
()
...
...
@@ -185,6 +189,7 @@ class TestCurves(unittest.TestCase):
return
def
test_piecewise_bezier_curve
(
self
):
print
(
"test_piecewise_bezier_curve"
)
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
...
...
@@ -208,6 +213,7 @@ class TestCurves(unittest.TestCase):
return
def
test_piecewise_cubic_hermite_curve
(
self
):
print
(
"test_piecewise_cubic_hermite_curve"
)
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
points
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
...
...
@@ -234,12 +240,12 @@ class TestCurves(unittest.TestCase):
return
def
test_exact_cubic
(
self
):
'''
print
(
"test_exact_cubic"
)
# To test :
# - Functions : constructor, min, max, derivate, getNumberSplines, getSplineAt
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
time_waypoints
=
matrix
([
0.
,
1.
]).
transpose
()
a = exact_cubic
3
(waypoints, time_waypoints)
a
=
exact_cubic
(
waypoints
,
time_waypoints
)
a
.
min
()
a
.
max
()
a
(
0.4
)
...
...
@@ -249,32 +255,33 @@ class TestCurves(unittest.TestCase):
a
.
getSplineAt
(
0
)
# Test serialization
a
.
saveAsText
(
"serialization_pc.test"
)
b = exact_cubic
3
()
b
=
exact_cubic
()
b
.
loadFromText
(
"serialization_pc.test"
)
self
.
assertTrue
((
a
(
0.4
)
==
b
(
0.4
)).
all
())
os
.
remove
(
"serialization_pc.test"
)
'''
return
def
test_exact_cubic_constraint
(
self
):
print
(
"test_exact_cubic_constraint"
)
# To test :
# - Functions : constructor, min, max, derivate
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
time_waypoints
=
matrix
([
0.
,
1.
]).
transpose
()
c
=
curve_constraints3
()
c
.
init_vel
c
.
end_vel
c
.
init_acc
c
.
end_acc
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.
,
1.
,
1.
]).
transpose
()
a
=
exact_cubic3
(
waypoints
,
time_waypoints
)
a
=
exact_cubic3
(
waypoints
,
time_waypoints
,
c
)
c
.
init_vel
c
.
end_vel
c
.
init_acc
c
.
end_acc
a
=
exact_cubic
(
waypoints
,
time_waypoints
)
a
=
exact_cubic
(
waypoints
,
time_waypoints
,
c
)
return
def
test_conversion_curves
(
self
):
print
(
"test_conversion_curves"
)
__EPS
=
1e-6
waypoints
=
matrix
([[
1.
,
2.
,
3.
],
[
4.
,
5.
,
6.
]]).
transpose
()
a
=
bezier
(
waypoints
)
...
...
tests/Main.cpp
View file @
5e0243f7
...
...
@@ -18,14 +18,13 @@ using namespace std;
namespace
curves
{
typedef
Eigen
::
Vector3d
point_t
;
typedef
Eigen
::
Vector3d
tangent_t
;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
curve_abc
<
double
,
double
,
true
,
point_t
>
curve_abc_t
;
typedef
polynomial
<
double
,
double
,
true
,
point_t
,
t_point_t
>
polynomial_t
;
typedef
exact_cubic
<
double
,
double
,
3
,
true
,
point_t
>
exact_cubic_t
;
typedef
exact_cubic
<
double
,
double
,
true
,
point_t
>
exact_cubic_t
;
typedef
exact_cubic
<
double
,
double
,
true
,
Eigen
::
Matrix
<
double
,
1
,
1
>
>
exact_cubic_one
;
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
,
true
,
point_t
,
t_point_t
,
polynomial_t
>
piecewise_polynomial_curve_t
;
typedef
piecewise_curve
<
double
,
double
,
true
,
point_t
,
t_point_t
,
bezier_curve_t
>
piecewise_bezier_curve_t
;
typedef
piecewise_curve
<
double
,
double
,
true
,
point_t
,
t_point_t
,
cubic_hermite_spline_t
>
piecewise_cubic_hermite_curve_t
;
...
...
@@ -35,7 +34,7 @@ namespace curves
typedef
Eigen
::
Matrix
<
double
,
1
,
1
>
point_one
;
typedef
std
::
pair
<
double
,
point_one
>
WaypointOne
;
typedef
std
::
vector
<
WaypointOne
>
T_WaypointOne
;
typedef
std
::
pair
<
point_t
,
tange
nt_t
>
pair_point_tangent_t
;
typedef
std
::
pair
<
point_t
,
poi
nt_t
>
pair_point_tangent_t
;
typedef
std
::
vector
<
pair_point_tangent_t
,
Eigen
::
aligned_allocator
<
pair_point_tangent_t
>
>
t_pair_point_tangent_t
;
const
double
margin
=
1e-3
;
...
...
@@ -630,12 +629,6 @@ void CheckWayPointConstraint(const std::string& errmsg, const double step, const
}
}
void
CheckDerivative
(
const
std
::
string
&
errmsg
,
const
double
eval_point
,
const
std
::
size_t
order
,
const
point_t
&
target
,
const
exact_cubic_t
*
curve
,
bool
&
error
)
{
point_t
res1
=
curve
->
derivate
(
eval_point
,
order
);
ComparePoints
(
target
,
res1
,
errmsg
,
error
);
}
void
ExactCubicPointsCrossedTest
(
bool
&
error
)
{
...
...
@@ -667,10 +660,10 @@ void ExactCubicVelocityConstraintsTest(bool& error)
CheckWayPointConstraint
(
errmsg
,
0.2
,
waypoints
,
&
exactCubic
,
error
);
std
::
string
errmsg3
(
"Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)"
);
// now check derivatives
C
heckDerivative
(
errmsg3
,
0
,
1
,
constraints
.
init_vel
,
&
exactCubic
,
error
);
C
heckDerivative
(
errmsg3
,
1
,
1
,
constraints
.
end_vel
,
&
exactCubic
,
error
);
C
heckDerivative
(
errmsg3
,
0
,
2
,
constraints
.
init_acc
,
&
exactCubic
,
error
);
C
heckDerivative
(
errmsg3
,
1
,
2
,
constraints
.
end_acc
,
&
exactCubic
,
error
);
C
omparePoints
(
constraints
.
init_vel
,
exactCubic
.
derivate
(
0
,
1
),
errmsg3
,
error
);
C
omparePoints
(
constraints
.
end_vel
,
exactCubic
.
derivate
(
1
,
1
),
errmsg3
,
error
);
C
omparePoints
(
constraints
.
init_acc
,
exactCubic
.
derivate
(
0
,
2
),
errmsg3
,
error
);
C
omparePoints
(
constraints
.
end_acc
,
exactCubic
.
derivate
(
1
,
2
),
errmsg3
,
error
);
constraints
.
end_vel
=
point_t
(
1
,
2
,
3
);
constraints
.
init_vel
=
point_t
(
-
1
,
-
2
,
-
3
);
constraints
.
end_acc
=
point_t
(
4
,
5
,
6
);
...
...
@@ -680,13 +673,14 @@ void ExactCubicVelocityConstraintsTest(bool& error)
CheckWayPointConstraint
(
errmsg2
,
0.2
,
waypoints
,
&
exactCubic2
,
error
);
std
::
string
errmsg4
(
"Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)"
);
// now check derivatives
C
heckDerivative
(
errmsg4
,
0
,
1
,
constraints
.
init_vel
,
&
exactCubic2
,
error
);
C
heckDerivative
(
errmsg4
,
1
,
1
,
constraints
.
end_vel
,
&
exactCubic2
,
error
);
C
heckDerivative
(
errmsg4
,
0
,
2
,
constraints
.
init_acc
,
&
exactCubic2
,
error
);
C
heckDerivative
(
errmsg4
,
1
,
2
,
constraints
.
end_acc
,
&
exactCubic2
,
error
);
C
omparePoints
(
constraints
.
init_vel
,
exactCubic2
.
derivate
(
0
,
1
),
errmsg4
,
error
);
C
omparePoints
(
constraints
.
end_vel
,
exactCubic2
.
derivate
(
1
,
1
),
errmsg4
,
error
);
C
omparePoints
(
constraints
.
init_acc
,
exactCubic2
.
derivate
(
0
,
2
),
errmsg4
,
error
);
C
omparePoints
(
constraints
.
end_acc
,
exactCubic2
.
derivate
(
1
,
2
),
errmsg4
,
error
);
}
void
CheckPointOnline
(
const
std
::
string
&
errmsg
,
const
point_t
&
A
,
const
point_t
&
B
,
const
double
target
,
const
exact_cubic_t
*
curve
,
bool
&
error
)
template
<
typename
CurveType
>
void
CheckPointOnline
(
const
std
::
string
&
errmsg
,
const
point_t
&
A
,
const
point_t
&
B
,
const
double
target
,
const
CurveType
*
curve
,
bool
&
error
)
{
point_t
res1
=
curve
->
operator
()(
target
);
point_t
ar
=
(
res1
-
A
);
ar
.
normalize
();
...
...
@@ -721,21 +715,20 @@ void EffectorTrajectoryTest(bool& error)
ComparePoints
(
off1
,
(
*
eff_traj
)(
1
),
errmsg
,
error
);
ComparePoints
(
off2
,
(
*
eff_traj
)(
9.5
),
errmsg
,
error
);
ComparePoints
(
end
,
(
*
eff_traj
)(
10
),
errmsg
,
error
);
//then check offset at start / goal positions
// now check derivatives
C
heckDerivative
(
errmsg2
,
0
,
1
,
zero
,
eff_traj
,
error
);
C
heckDerivative
(
errmsg2
,
10
,
1
,
zero
,
eff_traj
,
error
);
C
heckDerivative
(
errmsg2
,
0
,
2
,
zero
,
eff_traj
,
error
);
C
heckDerivative
(
errmsg2
,
10
,
2
,
zero
,
eff_traj
,
error
);
C
omparePoints
(
zero
,
(
*
eff_traj
).
derivate
(
0
,
1
),
errmsg2
,
error
);
C
omparePoints
(
zero
,
(
*
eff_traj
).
derivate
(
10
,
1
),
errmsg2
,
error
);