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
c89ae70e
Commit
c89ae70e
authored
Feb 13, 2020
by
Guilhem Saurel
Browse files
Merge remote-tracking branch 'main/devel' into devel
parents
7dac08b9
875a0ab4
Changes
18
Expand all
Hide whitespace changes
Inline
Side-by-side
include/curves/bernstein.h
View file @
c89ae70e
...
...
@@ -41,7 +41,9 @@ struct Bern {
~
Bern
()
{}
Numeric
operator
()(
const
Numeric
u
)
const
{
assert
(
u
>=
0.
&&
u
<=
1.
);
if
(
!
(
u
>=
0.
&&
u
<=
1.
))
{
throw
std
::
invalid_argument
(
"u needs to be betwen 0 and 1."
);
}
return
bin_m_i_
*
(
pow
(
u
,
i_
))
*
pow
((
1
-
u
),
m_minus_i
);
}
...
...
include/curves/bezier_curve.h
View file @
c89ae70e
...
...
@@ -66,10 +66,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
)),
degree_
(
size_
-
1
),
bernstein_
(
curves
::
makeBernstein
<
num_t
>
((
unsigned
int
)
degree_
))
{
assert
(
bernstein_
.
size
()
==
size_
);
if
(
bernstein_
.
size
()
!=
size_
)
{
throw
std
::
invalid_argument
(
"Invalid size of polynomial"
);
}
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
1
||
T_max_
<=
T_min_
))
{
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
// TODO
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
}
for
(;
it
!=
PointsEnd
;
++
it
)
{
control_points_
.
push_back
(
*
it
);
...
...
include/curves/cubic_hermite_spline.h
View file @
c89ae70e
...
...
@@ -233,7 +233,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
//
const
Time
dt
=
(
t1
-
t0
);
const
Time
alpha
=
(
t
-
t0
)
/
dt
;
assert
(
0.
<=
alpha
&&
alpha
<=
1.
&&
"alpha must be in [0,1]"
);
if
(
!
(
0.
<=
alpha
&&
alpha
<=
1.
))
{
throw
std
::
runtime_error
(
"alpha must be in [0,1]"
);
}
Numeric
h00
,
h10
,
h01
,
h11
;
evalCoeffs
(
alpha
,
h00
,
h10
,
h01
,
h11
,
degree_derivative
);
// std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<"
...
...
include/curves/helpers/effector_spline.h
View file @
c89ae70e
...
...
@@ -37,7 +37,9 @@ typedef exact_cubic_t::spline_t spline_t;
/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint
compute_offset
(
const
Waypoint
&
source
,
const
Point
&
normal
,
const
Numeric
offset
,
const
Time
time_offset
)
{
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
if
(
norm
<
0.
)
{
throw
std
::
runtime_error
(
"Norm of normal is less than 0!"
);
}
return
std
::
make_pair
(
source
.
first
+
time_offset
,
(
source
.
second
+
normal
/
norm
*
offset
));
}
...
...
@@ -46,7 +48,9 @@ Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numer
spline_t
make_end_spline
(
const
Point
&
normal
,
const
Point
&
from
,
const
Numeric
offset
,
const
Time
init_time
,
const
Time
time_offset
)
{
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
if
(
norm
<
0.
)
{
throw
std
::
runtime_error
(
"Norm of normal is less than 0!"
);
}
Point
n
=
normal
/
norm
;
Point
d
=
offset
/
(
time_offset
*
time_offset
*
time_offset
)
*
-
n
;
Point
c
=
-
3
*
d
*
time_offset
;
...
...
include/curves/optimization/OptimizeSpline.h
View file @
c89ae70e
...
...
@@ -38,7 +38,9 @@ struct SplineOptimizer {
///\brief Initializes optimizer environment.
SplineOptimizer
()
{
MSKrescodee
r_
=
MSK_makeenv
(
&
env_
,
NULL
);
assert
(
r_
==
MSK_RES_OK
);
if
(
r_
!=
MSK_RES_OK
)
{
throw
std
::
runtime_error
(
"Issue initializing MSK_makeenv"
);
}
}
///\brief Destructor.
...
...
include/curves/optimization/details.h
View file @
c89ae70e
...
...
@@ -174,8 +174,12 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
// add remaining variables (only if no end_pos constraints)
for
(;
i
<
numControlPoints
;
++
i
)
variables_
.
push_back
(
var_t
::
Zero
(
pDef
.
dim_
));
assert
(
numControlPoints
>
numConstants
);
assert
(
numControlPoints
==
variables_
.
size
());
if
(
numControlPoints
<=
numConstants
)
{
throw
std
::
runtime_error
(
"numControlPoints < numConstants"
);
}
if
(
numControlPoints
!=
variables_
.
size
())
{
throw
std
::
runtime_error
(
"numControlPoints != variables_.size()"
);
}
problemData
.
numControlPoints
=
numControlPoints
;
problemData
.
numVariables
=
numControlPoints
-
numConstants
;
...
...
@@ -246,8 +250,12 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
// compute sub-bezier curves
T_bezier_t
beziers
=
split
<
Point
,
Numeric
>
(
pDef
,
pData
);
assert
(
pDef
.
inequalityMatrices_
.
size
()
==
pDef
.
inequalityVectors_
.
size
());
assert
(
pDef
.
inequalityMatrices_
.
size
()
==
beziers
.
size
());
if
(
pDef
.
inequalityMatrices_
.
size
()
!=
pDef
.
inequalityVectors_
.
size
())
{
throw
std
::
invalid_argument
(
"The sizes of the inequality matrices and vectors do not match."
);
}
if
(
pDef
.
inequalityMatrices_
.
size
()
!=
beziers
.
size
())
{
throw
std
::
invalid_argument
(
"The sizes of the inequality matrices and the bezier degree do not match."
);
}
long
currentRowIdx
=
0
;
typename
problem_definition_t
::
CIT_matrix_x_t
cmit
=
pDef
.
inequalityMatrices_
.
begin
();
...
...
@@ -264,7 +272,7 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
currentRowIdx
+=
cmit
->
rows
();
}
}
assert
(
rows
==
currentRowIdx
);
// we filled all the constraints
assert
(
rows
==
currentRowIdx
);
// we filled all the constraints
- NB: leave assert for Debug tests
}
template
<
typename
Point
,
typename
Numeric
,
typename
In
>
...
...
@@ -273,8 +281,9 @@ quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1, In Po
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>
vector_x_t
;
unsigned
int
nPoints1
=
(
unsigned
int
)(
std
::
distance
(
PointsBegin1
,
PointsEnd1
)),
nPoints2
=
(
unsigned
int
)(
std
::
distance
(
PointsBegin2
,
PointsEnd2
));
assert
(
nPoints1
>
0
);
assert
(
nPoints2
>
0
);
if
(
nPoints1
<=
0
||
nPoints2
<=
0
)
{
throw
std
::
runtime_error
(
"This should never happen because an unsigned int cannot go negative without underflowing."
);
}
unsigned
int
deg1
=
nPoints1
-
1
,
deg2
=
nPoints2
-
1
;
unsigned
int
newDeg
=
(
deg1
+
deg2
);
// the integral of the primitive will simply be the last control points of the primitive,
...
...
include/curves/piecewise_curve.h
View file @
c89ae70e
...
...
@@ -212,9 +212,9 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
std
::
size_t
num_curves
()
const
{
return
curves_
.
size
();
}
const
curve_ptr_t
&
curve_at_time
(
const
time_t
t
)
const
{
return
curves_
[
find_interval
(
t
)];
}
curve_ptr_t
curve_at_time
(
const
time_t
t
)
const
{
return
curves_
[
find_interval
(
t
)];
}
const
curve_ptr_t
&
curve_at_index
(
const
std
::
size_t
idx
)
const
{
curve_ptr_t
curve_at_index
(
const
std
::
size_t
idx
)
const
{
if
(
Safe
&&
idx
>=
num_curves
())
{
throw
std
::
length_error
(
"curve_at_index: requested index greater than number of curves in piecewise_curve instance"
);
...
...
include/curves/polynomial.h
View file @
c89ae70e
...
...
@@ -106,6 +106,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
///
polynomial
(
const
Point
&
init
,
const
Point
&
end
,
const
time_t
min
,
const
time_t
max
)
:
dim_
(
init
.
size
()),
degree_
(
1
),
T_min_
(
min
),
T_max_
(
max
)
{
if
(
T_min_
>=
T_max_
)
throw
std
::
invalid_argument
(
"T_min must be strictly lower than T_max"
);
if
(
init
.
size
()
!=
end
.
size
())
throw
std
::
invalid_argument
(
"init and end points must have the same dimensions."
);
t_point_t
coeffs
;
coeffs
.
push_back
(
init
);
...
...
@@ -127,6 +128,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
polynomial
(
const
Point
&
init
,
const
Point
&
d_init
,
const
Point
&
end
,
const
Point
&
d_end
,
const
time_t
min
,
const
time_t
max
)
:
dim_
(
init
.
size
()),
degree_
(
3
),
T_min_
(
min
),
T_max_
(
max
)
{
if
(
T_min_
>=
T_max_
)
throw
std
::
invalid_argument
(
"T_min must be strictly lower than T_max"
);
if
(
init
.
size
()
!=
end
.
size
())
throw
std
::
invalid_argument
(
"init and end points must have the same dimensions."
);
if
(
init
.
size
()
!=
d_init
.
size
())
throw
std
::
invalid_argument
(
"init and d_init points must have the same dimensions."
);
...
...
@@ -170,6 +172,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
polynomial
(
const
Point
&
init
,
const
Point
&
d_init
,
const
Point
&
dd_init
,
const
Point
&
end
,
const
Point
&
d_end
,
const
Point
&
dd_end
,
const
time_t
min
,
const
time_t
max
)
:
dim_
(
init
.
size
()),
degree_
(
5
),
T_min_
(
min
),
T_max_
(
max
)
{
if
(
T_min_
>=
T_max_
)
throw
std
::
invalid_argument
(
"T_min must be strictly lower than T_max"
);
if
(
init
.
size
()
!=
end
.
size
())
throw
std
::
invalid_argument
(
"init and end points must have the same dimensions."
);
if
(
init
.
size
()
!=
d_init
.
size
())
throw
std
::
invalid_argument
(
"init and d_init points must have the same dimensions."
);
...
...
include/curves/quadratic_variable.h
View file @
c89ae70e
...
...
@@ -35,7 +35,9 @@ struct quadratic_variable {
}
quadratic_variable
(
const
matrix_x_t
&
A
,
const
point_t
&
b
,
const
Numeric
c
=
0
)
:
c_
(
c
),
b_
(
b
),
A_
(
A
),
zero
(
false
)
{
assert
(
A
.
cols
()
==
b
.
rows
()
&&
A
.
cols
()
==
A
.
rows
());
if
(
A
.
cols
()
!=
b
.
rows
()
||
A
.
cols
()
!=
A
.
rows
())
{
throw
std
::
invalid_argument
(
"The dimensions of A and b are incorrect."
);
}
}
quadratic_variable
(
const
point_t
&
b
,
const
Numeric
c
=
0
)
...
...
@@ -45,7 +47,9 @@ struct quadratic_variable {
// linear evaluation
Numeric
operator
()(
const
Eigen
::
Ref
<
const
point_t
>&
val
)
const
{
assert
(
!
isZero
());
if
(
isZero
())
{
throw
std
::
runtime_error
(
"Not initialized! (isZero)"
);
}
return
val
.
transpose
()
*
A
()
*
val
+
b
().
transpose
()
*
val
+
c
();
}
...
...
@@ -98,15 +102,21 @@ struct quadratic_variable {
}
const
matrix_x_t
&
A
()
const
{
assert
(
!
isZero
());
if
(
isZero
())
{
throw
std
::
runtime_error
(
"Not initialized! (isZero)"
);
}
return
A_
;
}
const
point_t
&
b
()
const
{
assert
(
!
isZero
());
if
(
isZero
())
{
throw
std
::
runtime_error
(
"Not initialized! (isZero)"
);
}
return
b_
;
}
const
Numeric
c
()
const
{
assert
(
!
isZero
());
if
(
isZero
())
{
throw
std
::
runtime_error
(
"Not initialized! (isZero)"
);
}
return
c_
;
}
bool
isZero
()
const
{
return
zero
;
}
...
...
include/curves/se3_curve.h
View file @
c89ae70e
...
...
@@ -25,9 +25,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
typedef
Eigen
::
Transform
<
Numeric
,
3
,
Eigen
::
Affine
>
transform_t
;
typedef
transform_t
point_t
;
typedef
Eigen
::
Matrix
<
Scalar
,
6
,
1
>
point_derivate_t
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
1
>
point3_t
;
typedef
Eigen
::
Matrix
<
Scalar
,
-
1
,
1
>
pointX_t
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
3
>
matrix3_t
;
typedef
Eigen
::
Quaternion
<
Scalar
>
Quaternion
;
typedef
Time
time_t
;
typedef
curve_abc
<
Time
,
Numeric
,
Safe
,
point_t
,
point_derivate_t
>
curve_abc_t
;
// parent class
...
...
@@ -215,6 +212,10 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual
std
::
size_t
degree
()
const
{
return
translation_curve_
->
degree
();
}
/// \brief const accessor to the translation curve
const
curve_ptr_t
translation_curve
()
const
{
return
translation_curve_
;}
/// \brief const accessor to the rotation curve
const
curve_rotation_ptr_t
rotation_curve
()
const
{
return
rotation_curve_
;}
/*Helpers*/
/*Attributes*/
...
...
include/curves/serialization/archive.hpp
View file @
c89ae70e
...
...
@@ -59,7 +59,9 @@ struct Serializable {
/// \brief Loads a Derived object from an XML file.
template
<
class
Derived
>
void
loadFromXML
(
const
std
::
string
&
filename
,
const
std
::
string
&
tag_name
)
{
assert
(
!
tag_name
.
empty
());
if
(
tag_name
.
empty
())
{
throw
std
::
invalid_argument
(
"tag_name cannot be empty."
);
}
std
::
ifstream
ifs
(
filename
.
c_str
());
if
(
ifs
)
{
boost
::
archive
::
xml_iarchive
ia
(
ifs
);
...
...
@@ -74,7 +76,9 @@ struct Serializable {
/// \brief Saved a Derived object as an XML file.
template
<
class
Derived
>
void
saveAsXML
(
const
std
::
string
&
filename
,
const
std
::
string
&
tag_name
)
const
{
assert
(
!
tag_name
.
empty
());
if
(
tag_name
.
empty
())
{
throw
std
::
invalid_argument
(
"tag_name cannot be empty."
);
}
std
::
ofstream
ofs
(
filename
.
c_str
());
if
(
ofs
)
{
boost
::
archive
::
xml_oarchive
oa
(
ofs
);
...
...
include/curves/so3_linear.h
View file @
c89ae70e
...
...
@@ -17,10 +17,8 @@ namespace curves {
///
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
bool
Safe
=
false
>
struct
SO3Linear
:
public
curve_abc
<
Time
,
Numeric
,
Safe
,
Eigen
::
Matrix
<
Numeric
,
3
,
3
>
,
Eigen
::
Matrix
<
Numeric
,
3
,
1
>
>
{
struct
SO3Linear
:
public
curve_abc
<
Time
,
Numeric
,
Safe
,
matrix3_t
,
point3_t
>
{
typedef
Numeric
Scalar
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
1
>
point3_t
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
3
>
matrix3_t
;
typedef
matrix3_t
point_t
;
typedef
point3_t
point_derivate_t
;
typedef
Eigen
::
Quaternion
<
Scalar
>
quaternion_t
;
...
...
@@ -40,7 +38,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
dim_
(
3
),
init_rot_
(
init_rot
),
end_rot_
(
end_rot
),
angular_vel_
(
log3
(
init_rot
.
toRotationMatrix
()
.
transpose
()
*
end_rot
.
toRotationMatrix
()
)
/
(
t_max
-
t_m
in
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
.
toRotationMatrix
()
,
end_rot
.
toRotationMatrix
()
,
t_min
,
t_m
ax
)),
T_min_
(
t_min
),
T_max_
(
t_max
)
{
safe_check
();
...
...
@@ -52,7 +50,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
dim_
(
3
),
init_rot_
(
quaternion_t
(
init_rot
)),
end_rot_
(
quaternion_t
(
end_rot
)),
angular_vel_
(
log3
(
init_rot
.
transpose
()
*
end_rot
)
/
(
t_max
-
t_m
in
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
,
end_rot
,
t_min
,
t_m
ax
)),
T_min_
(
t_min
),
T_max_
(
t_max
)
{
safe_check
();
...
...
@@ -64,7 +62,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
dim_
(
3
),
init_rot_
(
init_rot
),
end_rot_
(
end_rot
),
angular_vel_
(
log3
(
init_rot
.
toRotationMatrix
()
.
transpose
()
*
end_rot
.
toRotationMatrix
())),
angular_vel_
(
computeAngularVelocity
(
init_rot
.
toRotationMatrix
()
,
end_rot
.
toRotationMatrix
()
,
0.
,
1.
)),
T_min_
(
0.
),
T_max_
(
1.
)
{
safe_check
();
...
...
@@ -76,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
dim_
(
3
),
init_rot_
(
quaternion_t
(
init_rot
)),
end_rot_
(
quaternion_t
(
end_rot
)),
angular_vel_
(
log3
(
init_rot
.
toRotationMatrix
().
transpose
()
*
end_rot
.
toRotationMatrix
()
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
,
end_rot
,
0.
,
1.
)),
T_min_
(
0.
),
T_max_
(
1.
)
{
safe_check
();
...
...
@@ -95,12 +93,20 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
T_min_
(
other
.
T_min_
),
T_max_
(
other
.
T_max_
)
{}
point3_t
computeAngularVelocity
(
const
matrix3_t
&
init_rot
,
const
matrix3_t
&
end_rot
,
const
double
t_min
,
const
double
t_max
){
if
(
t_min
==
t_max
){
return
point3_t
::
Zero
();
}
else
{
return
log3
(
init_rot
.
transpose
()
*
end_rot
)
/
(
t_max
-
t_min
);
}
}
quaternion_t
computeAsQuaternion
(
const
time_t
t
)
const
{
if
(
Safe
&
!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
invalid_argument
(
"can't evaluate bezier curve, time t is out of range"
);
// TODO
}
if
(
t
>
T_max_
)
return
end_rot_
;
if
(
t
<
T_min_
)
return
init_rot_
;
if
(
t
>
=
T_max_
)
return
end_rot_
;
if
(
t
<
=
T_min_
)
return
init_rot_
;
Scalar
u
=
(
t
-
T_min_
)
/
(
T_max_
-
T_min_
);
return
init_rot_
.
slerp
(
u
,
end_rot_
);
}
...
...
@@ -108,7 +114,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \brief Evaluation of the SO3Linear at time t using Eigen slerp.
/// \param t : time when to evaluate the spline.
/// \return \f$x(t)\f$ point corresponding on spline at time t.
virtual
matrix3
_t
operator
()(
const
time_t
t
)
const
{
return
computeAsQuaternion
(
t
).
toRotationMatrix
();
}
virtual
point
_t
operator
()(
const
time_t
t
)
const
{
return
computeAsQuaternion
(
t
).
toRotationMatrix
();
}
/**
* @brief isApprox check if other and *this are approximately equals.
...
...
@@ -141,7 +147,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \param t : the time when to evaluate the spline.
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
virtual
point
3
_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
{
virtual
point
_derivate
_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
{
if
((
t
<
T_min_
||
t
>
T_max_
)
&&
Safe
)
{
throw
std
::
invalid_argument
(
"error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve"
);
...
...
@@ -250,7 +256,9 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
theta
=
PI_value
;
// acos((-1-1)/2)
else
theta
=
acos
((
tr
-
Scalar
(
1
))
/
Scalar
(
2
));
assert
(
theta
==
theta
&&
"theta contains some NaN"
);
// theta != NaN
if
(
!
std
::
isfinite
(
theta
))
{
throw
std
::
runtime_error
(
"theta contains some NaN"
);
}
// From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
if
(
theta
<
PI_value
-
1e-2
)
{
...
...
python/curves/CMakeLists.txt
View file @
c89ae70e
...
...
@@ -8,6 +8,15 @@ SET(${PY_NAME}_BINDINGS_SOURCES
namespace.h
)
SET
(
${
PROJECT_NAME
}
_PYTHON_HEADERS
python_definitions.h
)
INSTALL
(
FILES
${${
PROJECT_NAME
}
_PYTHON_HEADERS
}
DESTINATION include/
${
PROJECT_NAME
}
/python
)
ADD_LIBRARY
(
${
PY_NAME
}
SHARED
${${
PY_NAME
}
_BINDINGS_SOURCES
}
)
SET_TARGET_PROPERTIES
(
${
PY_NAME
}
PROPERTIES PREFIX
""
)
TARGET_COMPILE_OPTIONS
(
${
PY_NAME
}
PRIVATE
"-Wno-conversion"
)
...
...
python/curves/curves_python.cpp
View file @
c89ae70e
This diff is collapsed.
Click to expand it.
python/curves/python_definitions.h
View file @
c89ae70e
#include
"curves/fwd.h"
#include
"curves/bezier_curve.h"
#include
"curves/polynomial.h"
#include
"curves/linear_variable.h"
#include
"curves/quadratic_variable.h"
...
...
@@ -25,8 +26,8 @@ typedef std::vector<waypoint_t> t_waypoint_t;
typedef
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
point_listX_t
;
typedef
Eigen
::
Matrix
<
real
,
3
,
Eigen
::
Dynamic
>
point_list3_t
;
typedef
Eigen
::
Matrix
<
real
,
6
,
Eigen
::
Dynamic
>
point_list6_t
;
typedef
polynomial_t
::
coeff_t
coeff_t
;
typedef
curves
::
Bern
<
double
>
bernstein_t
;
template
<
typename
PointList
,
typename
T_Point
>
...
...
@@ -45,5 +46,16 @@ T_Point vectorFromEigenVector(const PointList& vector) {
}
return
res
;
}
template
<
typename
T_point
,
typename
PointList
>
PointList
vectorToEigenArray
(
const
T_point
&
vect
)
{
const
size_t
nCols
=
vect
.
size
();
const
size_t
nRows
=
vect
[
0
].
rows
();
PointList
res
(
nRows
,
nCols
);
for
(
size_t
i
=
0
;
i
<
vect
.
size
();
++
i
)
{
res
.
block
(
0
,
i
,
nRows
,
1
)
=
vect
[
i
];
}
return
res
;
}
}
// namespace curves
#endif //_DEFINITION_PYTHON_BINDINGS
python/curves/python_variables.cpp
View file @
c89ae70e
...
...
@@ -5,7 +5,9 @@
namespace
curves
{
std
::
vector
<
linear_variable_t
>
matrix3DFromEigenArray
(
const
point_list3_t
&
matrices
,
const
point_list3_t
&
vectors
)
{
assert
(
vectors
.
cols
()
*
3
==
matrices
.
cols
());
if
(
vectors
.
cols
()
*
3
!=
matrices
.
cols
())
{
throw
std
::
invalid_argument
(
"vectors.cols() * 3 != matrices.cols()"
);
}
std
::
vector
<
linear_variable_t
>
res
;
for
(
int
i
=
0
;
i
<
vectors
.
cols
();
++
i
)
{
res
.
push_back
(
linear_variable_t
(
matrices
.
block
<
3
,
3
>
(
0
,
i
*
3
),
vectors
.
col
(
i
)));
...
...
python/test/test.py
View file @
c89ae70e
...
...
@@ -98,6 +98,14 @@ class TestCurves(unittest.TestCase):
self
.
assertTrue
(
norm
(
a0
.
derivate
(
t
,
2
)
-
a1
.
derivate
(
3
*
t
,
2
)
*
9.
)
<
__EPS
)
self
.
assertTrue
(
norm
(
prim0
(
t
)
-
prim1
(
t
*
3
)
/
3.
)
<
__EPS
)
self
.
assertTrue
((
prim
(
0
)
==
array
([
0.
,
0.
,
0.
])).
all
())
# testing accessor to waypoints :
wp_getter
=
a0
.
waypoints
()
self
.
assertEqual
(
wp_getter
.
shape
[
0
],
waypoints
.
shape
[
0
])
self
.
assertEqual
(
wp_getter
.
shape
[
1
],
waypoints
.
shape
[
1
])
self
.
assertTrue
(
array_equal
(
wp_getter
,
waypoints
))
# check that it return a copy:
a0
.
waypoints
()[
1
,
1
]
=
-
15.
self
.
assertEqual
(
a0
.
waypoints
()[
1
,
1
],
waypoints
[
1
,
1
])
# testing bezier with constraints
c
=
curve_constraints
(
3
)
c
.
init_vel
=
array
([[
0.
,
1.
,
1.
]]).
transpose
()
...
...
@@ -189,6 +197,14 @@ class TestCurves(unittest.TestCase):
self
.
assertTrue
(
norm
(
a0
.
derivate
(
t
,
2
)
-
a1
.
derivate
(
3
*
t
,
2
)
*
9.
)
<
__EPS
)
self
.
assertTrue
(
norm
(
prim0
(
t
)
-
prim1
(
t
*
3
)
/
3.
)
<
__EPS
)
self
.
assertTrue
((
prim
(
0
)
==
array
([
0.
,
0.
,
0.
])).
all
())
# testing accessor to waypoints :
wp_getter
=
a0
.
waypoints
()
self
.
assertEqual
(
wp_getter
.
shape
[
0
],
waypoints
.
shape
[
0
])
self
.
assertEqual
(
wp_getter
.
shape
[
1
],
waypoints
.
shape
[
1
])
self
.
assertTrue
(
array_equal
(
wp_getter
,
waypoints
))
# check that it return a copy:
a0
.
waypoints
()[
1
,
1
]
=
-
15.
self
.
assertEqual
(
a0
.
waypoints
()[
1
,
1
],
waypoints
[
1
,
1
])
# testing bezier with constraints
c
=
curve_constraints
(
3
)
c
.
init_vel
=
array
([[
0.
,
1.
,
1.
]]).
transpose
()
...
...
@@ -344,6 +360,11 @@ class TestCurves(unittest.TestCase):
pc
.
derivate
(
0.4
,
2
)
pc
.
is_continuous
(
0
)
pc
.
is_continuous
(
1
)
a0
=
pc
.
curve_at_index
(
0
)
self
.
assertTrue
(
array_equal
(
a0
(
0.5
),
pc
(
0.5
)))
self
.
assertTrue
(
a0
==
a
)
a0
=
b
# should not have any effect
self
.
assertTrue
(
array_equal
(
pc
.
curve_at_index
(
0
)(
0.5
),
a
(
0.5
)))
# Test serialization
pc
.
saveAsText
(
"serialization_pc.test"
)
pc_test
=
piecewise
()
...
...
@@ -397,6 +418,13 @@ class TestCurves(unittest.TestCase):
for
i
in
range
(
N
):
self
.
assertTrue
(
isclose
(
polC0
(
time_points
[
i
,
0
]),
points
[:,
i
]).
all
())
c0
=
polC0
.
curve_at_index
(
0
)
self
.
assertEqual
(
c0
.
min
(),
time_points
[
0
])
self
.
assertEqual
(
c0
.
max
(),
time_points
[
1
])
self
.
assertEqual
(
c0
.
dim
(),
3
)
mid_t
=
(
c0
.
max
()
+
c0
.
min
())
/
2.
self
.
assertTrue
(
array_equal
(
polC0
(
mid_t
),
c0
(
mid_t
)))
polC1
=
piecewise
.
FromPointsList
(
points
,
points_derivative
,
time_points
)
self
.
assertEqual
(
polC1
.
min
(),
time_points
[
0
,
0
])
self
.
assertEqual
(
polC1
.
max
(),
time_points
[
-
1
,
0
])
...
...
@@ -448,6 +476,14 @@ class TestCurves(unittest.TestCase):
pc
.
derivate
(
0.4
,
2
)
pc
.
is_continuous
(
0
)
pc
.
is_continuous
(
1
)
# test access to curves :
self
.
assertTrue
(
array_equal
(
pc
.
curve_at_index
(
0
)(
0.5
),
a
(
0.5
)))
waypoints
=
array
([[
3.
,
4.
,
-
3.
],
[
5.
,
1.
,
2.
]]).
transpose
()
c
=
bezier
(
waypoints
,
1.5
,
2.
)
c0
=
pc
.
curve_at_index
(
0
)
c0
=
c
# should not have any effect
self
.
assertTrue
(
array_equal
(
pc
.
curve_at_index
(
0
)(
0.5
),
a
(
0.5
)))
# Test serialization
pc
.
saveAsText
(
"serialization_pc.test"
)
pc_test
=
piecewise
()
...
...
@@ -476,6 +512,11 @@ class TestCurves(unittest.TestCase):
pc
.
derivate
(
0.4
,
2
)
pc
.
is_continuous
(
0
)
pc
.
is_continuous
(
1
)
a0
=
pc
.
curve_at_index
(
0
)
self
.
assertTrue
(
array_equal
(
a0
(
0.5
),
pc
(
0.5
)))
self
.
assertTrue
(
a0
==
a
)
a0
=
b
# should not have any effect
self
.
assertTrue
(
array_equal
(
pc
.
curve_at_index
(
0
)(
0.5
),
a
(
0.5
)))
# Test serialization
pc
.
saveAsText
(
"serialization_pc.test"
)
pc_test
=
piecewise
()
...
...
@@ -897,6 +938,18 @@ class TestCurves(unittest.TestCase):
self
.
assertTrue
(
isclose
(
d
,
se3
.
derivate
(
max
,
1
)).
all
())
self
.
assertTrue
(
isclose
(
se3
.
derivate
(
min
,
2
),
zeros
((
6
,
1
))).
all
())
self
.
assertTrue
(
isclose
(
se3
.
derivate
(
min
,
3
),
zeros
((
6
,
1
))).
all
())
# test accessor to translation_curve :
tr_se3
=
se3
.
translation_curve
()
self
.
assertTrue
(
array_equal
(
tr_se3
((
max
+
min
)
/
2.
),
se3
.
translation
((
max
+
min
)
/
2.
)))
# test accessor to rotation :
rot_se3
=
se3
.
rotation_curve
()
rot_se3
((
max
+
min
)
/
2.
)
self
.
assertTrue
(
isclose
(
rot_se3
((
max
+
min
)
/
2.
),(
se3
.
rotation
((
max
+
min
)
/
2.
))).
all
())
# check that it return a CONST reference :
waypoints2
=
array
([[
1.
,
-
2.
,
3.5
],
[
5.6
,
5.
,
-
6.
],
[
4.
,
1.2
,
0.5
]]).
transpose
()
tr_se3
=
bezier3
(
waypoints2
,
min
,
max
)
# should not have any effect
self
.
assertFalse
(
array_equal
(
tr_se3
((
max
+
min
)
/
2.
),
se3
.
translation
((
max
+
min
)
/
2.
)))
# check that errors are correctly raised when necessary :
with
self
.
assertRaises
(
ValueError
):
se3
(
0.
)
...
...
@@ -949,6 +1002,21 @@ class TestCurves(unittest.TestCase):
self
.
assertTrue
(
isclose
(
se3
.
derivate
(
t
,
1
)[
0
:
3
],
translation
.
derivate
(
t
,
1
)).
all
())
t
+=
0.02
# test accessor to translation_curve :
tr_se3
=
se3
.
translation_curve
()
self
.
assertTrue
(
tr_se3
==
translation
)
self
.
assertTrue
(
array_equal
(
tr_se3
((
max
+
min
)
/
2.
),
se3
.
translation
((
max
+
min
)
/
2.
)))
# test accessor to rotation :
rot_se3
=
se3
.
rotation_curve
()
rot_se3
((
max
+
min
)
/
2.
)
se3
.
rotation
((
max
+
min
)
/
2.
)
self
.
assertTrue
(
isclose
(
rot_se3
((
max
+
min
)
/
2.
),
se3
.
rotation
((
max
+
min
)
/
2.
)).
all
())
# check that it return a CONST reference :
waypoints2
=
array
([[
1.
,
-
2.
,
3.5
],
[
5.6
,
5.
,
-
6.
],
[
4.
,
1.2
,
0.5
]]).
transpose
()
tr_se3
=
bezier3
(
waypoints2
,
min
,
max
)
# should not have any effect
self
.
assertFalse
(
array_equal
(
tr_se3
((
max
+
min
)
/
2.
),
se3
.
translation
((
max
+
min
)
/
2.
)))
# test with bezier3
translation
=
bezier3
(
waypoints
,
min
,
max
)
se3
=
SE3Curve
(
translation
,
init_rot
,
end_rot
)
...
...
@@ -1051,6 +1119,20 @@ class TestCurves(unittest.TestCase):
self
.
assertTrue
(
isclose
(
se3
.
derivate
(
t
,
1
)[
3
:
6
],
rotation
.
derivate
(
t
,
1
)).
all
())
t
+=
0.02
# test accessor to translation_curve :
tr_se3
=
se3
.
translation_curve
()
self
.
assertTrue
(
tr_se3
==
translation
)
self
.
assertTrue
(
array_equal
(
tr_se3
((
max
+
min
)
/
2.
),
se3
.
translation
((
max
+
min
)
/
2.
)))
# test accessor to rotation :
rot_se3
=
se3
.
rotation_curve
()
rot_se3
((
max
+
min
)
/
2.
)
se3
.
rotation
((
max
+
min
)
/
2.
)
self
.
assertTrue
(
array_equal
(
rot_se3
((
max
+
min
)
/
2.
),
se3
.
rotation
((
max
+
min
)
/
2.
)))
# check that it return a CONST reference :
waypoints2
=
array
([[
1.
,
-
2.
,
3.5
],
[
5.6
,
5.
,
-
6.
],
[
4.
,
1.2
,
0.5
]]).
transpose
()
tr_se3
=
bezier3
(
waypoints2
,
min
,
max
)
# should not have any effect
self
.
assertFalse
(
array_equal
(
tr_se3
((
max
+
min
)
/
2.
),
se3
.
translation
((
max
+
min
)
/
2.
)))
# check if errors are correctly raised :
rotation
=
SO3Linear
(
init_rot
,
end_rot
,
min
+
0.2
,
max
)
with
self
.
assertRaises
(
ValueError
):
...
...
tests/Main.cpp
View file @
c89ae70e
...
...
@@ -1855,6 +1855,36 @@ void se3CurveTest(bool& error) {
std
::
cout
<<
"SE3 curve : second order derivative for rotation should be null"
<<
std
::
endl
;
}
// check accessor to translation curves :
curve_ptr_t
translation
=
cBezier
.
translation_curve
();
if
(
translation
->
operator
()(
min
)
!=
cBezier
(
min
).
translation
())
{
error
=
true
;
std
::
cout
<<
"SE3 curve : translation curve not equal to se3.translation"
<<
std
::
endl
;
}
if
(
translation
->
operator
()(
max
)
!=
cBezier
(
max
).
translation
())
{
error
=
true
;
std
::
cout
<<
"SE3 curve : translation curve not equal to se3.translation"
<<
std
::
endl
;
}
if
(
translation
->
operator
()((
max
+
min
)
/
2.
)
!=
cBezier
((
max
+
min
)
/
2.
).
translation
())
{
error
=
true
;
std
::
cout
<<
"SE3 curve : translation curve not equal to se3.translation"
<<
std
::
endl
;
}
// check accessor to rotation curves :
curve_rotation_ptr_t
rotation
=
cBezier
.
rotation_curve
();
if
(
!
rotation
->
operator
()(
min
).
isApprox
(
cBezier
(
min
).
rotation
()))
{
error
=
true
;
std
::
cout
<<
"SE3 curve : rotation curve not equal to se3.rotation"
<<
std
::
endl
;
}
if
(
!
rotation
->
operator
()(
max
).
isApprox
(
cBezier
(
max
).
rotation
()))
{
error
=
true
;
std
::
cout
<<
"SE3 curve : rotation curve not equal to se3.rotation"
<<
std
::
endl
;