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
8d201862
Unverified
Commit
8d201862
authored
Jan 31, 2020
by
Fernbach Pierre
Committed by
GitHub
Jan 31, 2020
Browse files
Merge pull request #30 from pFernbach/devel
Add API to get bezier waypoints and SE3 translation/rotation curves
parents
3092691d
6cd9a639
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/curves/se3_curve.h
View file @
8d201862
...
...
@@ -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/so3_linear.h
View file @
8d201862
...
...
@@ -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
;
...
...
@@ -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_
(
log3
(
init_rot
.
transpose
()
*
end_rot
)),
T_min_
(
0.
),
T_max_
(
1.
)
{
safe_check
();
...
...
@@ -108,7 +106,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 +139,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"
);
...
...
python/curves/curves_python.cpp
View file @
8d201862
...
...
@@ -24,8 +24,8 @@ struct CurveWrapper : curve_abc_t, wrapper<curve_abc_t> {
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
(
curve_abc_t_isEquivalent_overloads
,
curve_abc_t
::
isEquivalent
,
1
,
3
)
struct
Curve3Wrapper
:
curve_3_t
,
wrapper
<
curve_3_t
>
{
point_t
operator
()(
const
real
)
{
return
this
->
get_override
(
"operator()"
)();
}
point_t
derivate
(
const
real
,
const
std
::
size_t
)
{
return
this
->
get_override
(
"derivate"
)();
}
point
3
_t
operator
()(
const
real
)
{
return
this
->
get_override
(
"operator()"
)();
}
point
3
_t
derivate
(
const
real
,
const
std
::
size_t
)
{
return
this
->
get_override
(
"derivate"
)();
}
curve_t
*
compute_derivate_ptr
(
const
real
)
{
return
this
->
get_override
(
"compute_derivate"
)();
}
std
::
size_t
dim
()
{
return
this
->
get_override
(
"dim"
)();
}
real
min
()
{
return
this
->
get_override
(
"min"
)();
}
...
...
@@ -34,8 +34,8 @@ struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t> {
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
(
curve_3_t_isEquivalent_overloads
,
curve_3_t
::
isEquivalent
,
1
,
3
)
struct
CurveRotationWrapper
:
curve_rotation_t
,
wrapper
<
curve_rotation_t
>
{
point_t
operator
()(
const
real
)
{
return
this
->
get_override
(
"operator()"
)();
}
point
_t
derivate
(
const
real
,
const
std
::
size_t
)
{
return
this
->
get_override
(
"derivate"
)();
}
curve_rotation_t
::
point_t
operator
()(
const
real
)
{
return
this
->
get_override
(
"operator()"
)();
}
curve_rotation_t
::
point_derivate
_t
derivate
(
const
real
,
const
std
::
size_t
)
{
return
this
->
get_override
(
"derivate"
)();
}
curve_t
*
compute_derivate_ptr
(
const
real
)
{
return
this
->
get_override
(
"compute_derivate"
)();
}
std
::
size_t
dim
()
{
return
this
->
get_override
(
"dim"
)();
}
real
min
()
{
return
this
->
get_override
(
"min"
)();
}
...
...
@@ -44,8 +44,8 @@ struct CurveRotationWrapper : curve_rotation_t, wrapper<curve_rotation_t> {
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
(
curve_rotation_t_isEquivalent_overloads
,
curve_rotation_t
::
isEquivalent
,
1
,
3
)
struct
CurveSE3Wrapper
:
curve_SE3_t
,
wrapper
<
curve_SE3_t
>
{
point_t
operator
()(
const
real
)
{
return
this
->
get_override
(
"operator()"
)();
}
point
_t
derivate
(
const
real
,
const
std
::
size_t
)
{
return
this
->
get_override
(
"derivate"
)();
}
curve_SE3_t
::
point_t
operator
()(
const
real
)
{
return
this
->
get_override
(
"operator()"
)();
}
curve_SE3_t
::
point_derivate
_t
derivate
(
const
real
,
const
std
::
size_t
)
{
return
this
->
get_override
(
"derivate"
)();
}
curve_t
*
compute_derivate_ptr
(
const
real
)
{
return
this
->
get_override
(
"compute_derivate"
)();
}
std
::
size_t
dim
()
{
return
this
->
get_override
(
"dim"
)();
}
real
min
()
{
return
this
->
get_override
(
"min"
)();
}
...
...
@@ -96,6 +96,10 @@ bezier3_t* wrapBezier3ConstructorBoundsConstraints(const pointX_list_t& array, c
return
wrapBezierConstructorConstraintsTemplate
<
bezier3_t
,
pointX_list_t
,
t_point3_t
,
curve_constraints3_t
>
(
array
,
convertToConstraints3
(
constraints
),
T_min
,
T_max
);
}
pointX_list_t
wrapBezier3Waypoints
(
bezier3_t
&
self
){
return
vectorToEigenArray
<
bezier3_t
::
t_point_t
,
pointX_list_t
>
(
self
.
waypoints
());
}
/*END 3D constructors bezier */
/*constructors bezier */
...
...
@@ -114,6 +118,9 @@ bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, con
return
wrapBezierConstructorConstraintsTemplate
<
bezier_t
,
pointX_list_t
,
t_pointX_t
,
curve_constraints_t
>
(
array
,
constraints
,
T_min
,
T_max
);
}
pointX_list_t
wrapBezierWaypoints
(
bezier_t
&
self
){
return
vectorToEigenArray
<
bezier_t
::
t_point_t
,
pointX_list_t
>
(
self
.
waypoints
());
}
/*END constructors bezier */
/* Wrap Cubic hermite spline */
...
...
@@ -349,6 +356,14 @@ SE3Curve_t* wrapSE3CurveFromTwoCurves(const curve_ptr_t& translation_curve,
return
new
SE3Curve_t
(
translation_curve
,
rotation_curve
);
}
curve_abc_t
*
SE3getTranslationCurve
(
SE3Curve_t
&
self
){
return
self
.
translation_curve
().
get
();
}
curve_rotation_t
*
SE3getRotationCurve
(
SE3Curve_t
&
self
){
return
self
.
rotation_curve
().
get
();
}
#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
typedef
pinocchio
::
SE3Tpl
<
real
,
0
>
SE3_t
;
typedef
pinocchio
::
MotionTpl
<
real
,
0
>
Motion_t
;
...
...
@@ -417,7 +432,7 @@ BOOST_PYTHON_MODULE(curves) {
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
class_
<
CurveWrapper
,
boost
::
noncopyable
>
(
"curve"
,
no_init
)
class_
<
CurveWrapper
,
boost
::
noncopyable
,
boost
::
shared_ptr
<
curve_abc_t
>
>
(
"curve"
,
no_init
)
.
def
(
"__call__"
,
pure_virtual
(
&
curve_abc_t
::
operator
()),
"Evaluate the curve at the given time."
,
args
(
"self"
,
"t"
))
.
def
(
"derivate"
,
pure_virtual
(
&
curve_abc_t
::
derivate
),
"Evaluate the derivative of order N of curve at time t."
,
...
...
@@ -445,7 +460,7 @@ BOOST_PYTHON_MODULE(curves) {
.
def
(
"loadFromBinary"
,
pure_virtual
(
&
curve_abc_t
::
loadFromBinary
<
curve_abc_t
>
),
bp
::
args
(
"filename"
),
"Loads *this from a binary file."
);
class_
<
Curve3Wrapper
,
boost
::
noncopyable
,
bases
<
curve_abc_t
>
>
(
"curve3"
,
no_init
)
class_
<
Curve3Wrapper
,
boost
::
noncopyable
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
curve_3_t
>
>
(
"curve3"
,
no_init
)
.
def
(
"__call__"
,
pure_virtual
(
&
curve_3_t
::
operator
()),
"Evaluate the curve at the given time."
,
args
(
"self"
,
"t"
))
.
def
(
"derivate"
,
pure_virtual
(
&
curve_3_t
::
derivate
),
"Evaluate the derivative of order N of curve at time t."
,
...
...
@@ -461,7 +476,7 @@ BOOST_PYTHON_MODULE(curves) {
.
def
(
"max"
,
pure_virtual
(
&
curve_3_t
::
max
),
"Get the HIGHER bound on interval definition of the curve."
)
.
def
(
"dim"
,
pure_virtual
(
&
curve_3_t
::
dim
),
"Get the dimension of the curve."
);
class_
<
CurveRotationWrapper
,
boost
::
noncopyable
,
bases
<
curve_abc_t
>
>
(
"curve_rotation"
,
no_init
)
class_
<
CurveRotationWrapper
,
boost
::
noncopyable
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
curve_rotation_t
>
>
(
"curve_rotation"
,
no_init
)
.
def
(
"__call__"
,
pure_virtual
(
&
curve_rotation_t
::
operator
()),
"Evaluate the curve at the given time."
,
args
(
"self"
,
"t"
))
.
def
(
"derivate"
,
pure_virtual
(
&
curve_rotation_t
::
derivate
),
...
...
@@ -477,7 +492,7 @@ BOOST_PYTHON_MODULE(curves) {
.
def
(
"max"
,
pure_virtual
(
&
curve_rotation_t
::
max
),
"Get the HIGHER bound on interval definition of the curve."
)
.
def
(
"dim"
,
pure_virtual
(
&
curve_rotation_t
::
dim
),
"Get the dimension of the curve."
);
class_
<
CurveSE3Wrapper
,
boost
::
noncopyable
,
bases
<
curve_abc_t
>
>
(
"curve_SE3"
,
no_init
)
class_
<
CurveSE3Wrapper
,
boost
::
noncopyable
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
curve_SE3_t
>
>
(
"curve_SE3"
,
no_init
)
.
def
(
"__call__"
,
&
se3Return
,
"Evaluate the curve at the given time. Return as an homogeneous matrix."
,
args
(
"self"
,
"t"
))
.
def
(
"derivate"
,
pure_virtual
(
&
curve_SE3_t
::
derivate
),
...
...
@@ -513,6 +528,7 @@ BOOST_PYTHON_MODULE(curves) {
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezier3ConstructorBoundsConstraints
))
.
def
(
"compute_primitive"
,
&
bezier3_t
::
compute_primitive
)
.
def
(
"waypointAtIndex"
,
&
bezier3_t
::
waypointAtIndex
)
.
def
(
"waypoints"
,
&
wrapBezier3Waypoints
)
.
def_readonly
(
"degree"
,
&
bezier3_t
::
degree_
)
.
def_readonly
(
"nbWaypoints"
,
&
bezier3_t
::
size_
)
.
def
(
"saveAsText"
,
&
bezier3_t
::
saveAsText
<
bezier3_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a text file."
)
...
...
@@ -537,6 +553,7 @@ BOOST_PYTHON_MODULE(curves) {
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierConstructorBoundsConstraints
))
.
def
(
"compute_primitive"
,
&
bezier_t
::
compute_primitive
)
.
def
(
"waypointAtIndex"
,
&
bezier_t
::
waypointAtIndex
)
.
def
(
"waypoints"
,
&
wrapBezierWaypoints
)
.
def_readonly
(
"degree"
,
&
bezier_t
::
degree_
)
.
def_readonly
(
"nbWaypoints"
,
&
bezier_t
::
size_
)
.
def
(
"split"
,
splitspe
)
...
...
@@ -605,7 +622,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END variable points bezier curve**/
/** BEGIN polynomial curve function**/
class_
<
polynomial_t
,
bases
<
curve_abc_t
>
>
(
"polynomial"
,
init
<>
())
class_
<
polynomial_t
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
polynomial_t
>
>
(
"polynomial"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapPolynomialConstructor1
,
default_call_policies
(),
args
(
"coeffs"
,
"min"
,
"max"
)),
"Create polynomial spline from an Eigen matrix of coefficient defined for t in [min,max]."
...
...
@@ -654,7 +671,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END polynomial function**/
/** BEGIN piecewise curve function **/
class_
<
piecewise_t
,
bases
<
curve_abc_t
>
>
(
"piecewise"
,
init
<>
())
class_
<
piecewise_t
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
piecewise_t
>
>
(
"piecewise"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapPiecewiseCurveConstructor
,
default_call_policies
(),
arg
(
"curve"
)),
"Create a peicewise curve containing the given curve."
)
.
def
(
"FromPointsList"
,
&
discretPointToPolynomialC0
,
...
...
@@ -714,7 +731,7 @@ BOOST_PYTHON_MODULE(curves) {
.
def
(
bp
::
self
==
bp
::
self
)
.
def
(
bp
::
self
!=
bp
::
self
);
class_
<
piecewise_bezier_t
,
bases
<
curve_abc_t
>
>
(
"piecewise_bezier"
,
init
<>
())
class_
<
piecewise_bezier_t
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
piecewise_bezier_t
>
>
(
"piecewise_bezier"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapPiecewiseBezierConstructor
,
default_call_policies
(),
arg
(
"curve"
)),
"Create a peicewise Bezier curve containing the given curve."
)
.
def
(
"__init__"
,
make_constructor
(
&
wrapPiecewiseBezierEmptyConstructor
),
...
...
@@ -742,7 +759,7 @@ BOOST_PYTHON_MODULE(curves) {
.
def
(
bp
::
self
==
bp
::
self
)
.
def
(
bp
::
self
!=
bp
::
self
);
class_
<
piecewise_linear_bezier_t
,
bases
<
curve_abc_t
>
>
(
"piecewise_bezier_linear"
,
init
<>
())
class_
<
piecewise_linear_bezier_t
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
piecewise_linear_bezier_t
>
>
(
"piecewise_bezier_linear"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapPiecewiseBezierLinearConstructor
,
default_call_policies
(),
arg
(
"curve"
)),
"Create a peicewise Bezier curve containing the given curve."
)
.
def
(
"__init__"
,
make_constructor
(
&
wrapPiecewiseBezierLinearEmptyConstructor
),
...
...
@@ -770,7 +787,7 @@ BOOST_PYTHON_MODULE(curves) {
.
def
(
bp
::
self
==
bp
::
self
)
.
def
(
bp
::
self
!=
bp
::
self
);
class_
<
piecewise_SE3_t
,
bases
<
curve_SE3_t
>
>
(
"piecewise_SE3"
,
init
<>
())
class_
<
piecewise_SE3_t
,
bases
<
curve_SE3_t
>
,
boost
::
shared_ptr
<
piecewise_SE3_t
>
>
(
"piecewise_SE3"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapPiecewiseSE3Constructor
,
default_call_policies
(),
arg
(
"curve"
)),
"Create a piecewise-se3 curve containing the given se3 curve."
)
.
def
(
"__init__"
,
make_constructor
(
&
wrapPiecewiseSE3EmptyConstructor
),
"Create an empty piecewise-se3 curve."
)
...
...
@@ -811,7 +828,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END piecewise curve function **/
/** BEGIN exact_cubic curve**/
class_
<
exact_cubic_t
,
bases
<
curve_abc_t
>
>
(
"exact_cubic"
,
init
<>
())
class_
<
exact_cubic_t
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
exact_cubic_t
>
>
(
"exact_cubic"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapExactCubicConstructor
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapExactCubicConstructorConstraint
))
.
def
(
"getNumberSplines"
,
&
exact_cubic_t
::
getNumberSplines
)
...
...
@@ -833,7 +850,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END exact_cubic curve**/
/** BEGIN cubic_hermite_spline **/
class_
<
cubic_hermite_spline_t
,
bases
<
curve_abc_t
>
>
(
"cubic_hermite_spline"
,
init
<>
())
class_
<
cubic_hermite_spline_t
,
bases
<
curve_abc_t
>
,
boost
::
shared_ptr
<
cubic_hermite_spline_t
>
>
(
"cubic_hermite_spline"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapCubicHermiteSplineConstructor
))
.
def
(
"saveAsText"
,
&
cubic_hermite_spline_t
::
saveAsText
<
cubic_hermite_spline_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a text file."
)
...
...
@@ -868,7 +885,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END bernstein polynomial**/
/** BEGIN SO3 Linear**/
class_
<
SO3Linear_t
,
bases
<
curve_rotation_t
>
>
(
"SO3Linear"
,
init
<>
())
class_
<
SO3Linear_t
,
bases
<
curve_rotation_t
>
,
boost
::
shared_ptr
<
SO3Linear_t
>
>
(
"SO3Linear"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapSO3LinearConstructorFromMatrix
,
default_call_policies
(),
args
(
"init_rotation"
,
"end_rotation"
,
"min"
,
"max"
)),
...
...
@@ -899,7 +916,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END SO3 Linear**/
/** BEGIN SE3 Curve**/
class_
<
SE3Curve_t
,
bases
<
curve_SE3_t
>
>
(
"SE3Curve"
,
init
<>
())
class_
<
SE3Curve_t
,
bases
<
curve_SE3_t
>
,
boost
::
shared_ptr
<
SE3Curve_t
>
>
(
"SE3Curve"
,
init
<>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapSE3CurveFromTransform
,
default_call_policies
(),
args
(
"init_transform"
,
"end_transform"
,
"min"
,
"max"
)),
...
...
@@ -935,6 +952,10 @@ BOOST_PYTHON_MODULE(curves) {
"translation curve."
"The orientation along the SE3Curve will be a slerp between the two given rotations."
"The orientations should be represented as 3x3 rotation matrix"
)
.
def
(
"translation_curve"
,
&
SE3getTranslationCurve
,
return_internal_reference
<>
(),
"Return a curve corresponding to the translation part of self."
)
.
def
(
"rotation_curve"
,
&
SE3getRotationCurve
,
return_internal_reference
<>
(),
"Return a curve corresponding to the rotation part of self."
)
.
def
(
"saveAsText"
,
&
SE3Curve_t
::
saveAsText
<
SE3Curve_t
>
,
bp
::
args
(
"filename"
),
"Saves *this inside a text file."
)
.
def
(
"loadFromText"
,
&
SE3Curve_t
::
loadFromText
<
SE3Curve_t
>
,
bp
::
args
(
"filename"
),
"Loads *this from a text file."
)
...
...
python/curves/python_definitions.h
View file @
8d201862
...
...
@@ -45,5 +45,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/test/test.py
View file @
8d201862
...
...
@@ -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
()
...
...
@@ -448,6 +464,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
()
...
...
@@ -897,6 +921,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 +985,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 +1102,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 @
8d201862
...
...
@@ -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
;
}
if
(
!
rotation
->
operator
()((
max
+
min
)
/
2.
).
isApprox
(
cBezier
((
max
+
min
)
/
2.
).
rotation
()))
{
error
=
true
;
std
::
cout
<<
"SE3 curve : rotation curve not equal to se3.rotation"
<<
std
::
endl
;
}
// check if errors are correctly raised
try
{
cBezier
(
0.1
);
...
...
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