1 #ifndef _STRUCT_SE3_CURVE_H 2 #define _STRUCT_SE3_CURVE_H 8 #include <boost/math/constants/constants.hpp> 21 template <
typename Time =
double,
typename Numeric = Time,
bool Safe = false>
22 struct SE3Curve :
public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric, 3, Eigen::Affine>,
23 Eigen::Matrix<Numeric, 6, 1> > {
25 typedef Eigen::Transform<Numeric, 3, Eigen::Affine>
transform_t;
56 SE3Curve(
const transform_t& init_transform,
const transform_t& end_transform,
const time_t& t_min,
61 pointX_t(end_transform.translation()), t_min, t_max)),
62 rotation_curve_(new SO3Linear_t(init_transform.rotation(), end_transform.rotation(), t_min, t_max)),
71 const time_t& t_min,
const time_t& t_max)
84 const time_t& t_min,
const time_t& t_max)
101 rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->
min(), translation_curve->
max())),
112 rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->
min(), translation_curve->
max())),
127 if (translation_curve->dim() != 3) {
128 throw std::invalid_argument(
"The translation curve should be of dimension 3.");
130 if (rotation_curve->min() !=
T_min_) {
131 throw std::invalid_argument(
"Min bounds of translation and rotation curve are not the same.");
133 if (rotation_curve->max() !=
T_max_) {
134 throw std::invalid_argument(
"Max bounds of translation and rotation curve are not the same.");
144 throw std::invalid_argument(
"Translation curve should always be of dimension 3");
146 point_t res = point_t::Identity();
160 bool isApprox(
const SE3Curve_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
161 return ndcurves::isApprox<Numeric>(
T_min_, other.
min()) && ndcurves::isApprox<Numeric>(
T_max_, other.
max()) &&
168 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
169 const SE3Curve_t* other_cast =
dynamic_cast<const SE3Curve_t*
>(other);
178 virtual bool operator!=(
const SE3Curve_t& other)
const {
return !(*
this == other); }
184 virtual point_derivate_t
derivate(
const time_t t,
const std::size_t order)
const {
186 throw std::invalid_argument(
"Translation curve should always be of dimension 3");
188 point_derivate_t res = point_derivate_t::Zero();
195 throw std::logic_error(
"Compute derivate for SE3 is not implemented yet.");
206 std::size_t
virtual dim()
const {
return dim_; };
232 template <
class Archive>
233 void serialize(Archive& ar,
const unsigned int version) {
237 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
238 ar& boost::serialization::make_nvp(
"dim", dim_);
239 ar& boost::serialization::make_nvp(
"translation_curve", translation_curve_);
240 ar& boost::serialization::make_nvp(
"rotation_curve", rotation_curve_);
241 ar& boost::serialization::make_nvp(
"T_min", T_min_);
242 ar& boost::serialization::make_nvp(
"T_max", T_max_);
248 if (T_min_ > T_max_) {
249 throw std::invalid_argument(
"Tmin should be inferior to Tmax");
251 if (translation_curve_->dim() != 3) {
252 throw std::invalid_argument(
"Translation curve should always be of dimension 3");
261 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe),
264 #endif // SE3_CURVE_H Definition: bernstein.h:20
Eigen::Transform< Numeric, 3, Eigen::Affine > transform_t
Definition: se3_curve.h:25
curve_abc< Time, Numeric, Safe, pointX_t > curve_X_t
Definition: se3_curve.h:32
const curve_ptr_t translation_curve() const
const accessor to the translation curve
Definition: se3_curve.h:217
std::size_t dim_
Definition: se3_curve.h:223
SE3Curve(const pointX_t &init_pos, const pointX_t &end_pos, const matrix3_t &init_rot, const matrix3_t &end_rot, const time_t &t_min, const time_t &t_max)
Constructor from init/end pose, with rotation matrix. use polynomial of degree 1 for position and SO3...
Definition: se3_curve.h:83
time_t max() const
Get the maximum time for which the curve is defined.
Definition: se3_curve.h:212
Numeric Scalar
Definition: se3_curve.h:24
transform_t point_t
Definition: se3_curve.h:26
Eigen::VectorXd pointX_t
Definition: fwd.h:64
time_t T_max_
Definition: se3_curve.h:226
virtual std::size_t degree() const
Get the degree of the curve.
Definition: se3_curve.h:215
SE3Curve(const transform_t &init_transform, const transform_t &end_transform, const time_t &t_min, const time_t &t_max)
Constructor from init/end transform use polynomial of degree 1 for position and SO3Linear for rotatio...
Definition: se3_curve.h:56
time_t min() const
Get the minimum time for which the curve is defined.
Definition: se3_curve.h:209
SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)
Constructor from from translation and rotation curves object.
Definition: se3_curve.h:120
~SE3Curve()
Destructor.
Definition: se3_curve.h:49
virtual point_t operator()(const time_t t) const
Evaluation of the SE3Curve at time t.
Definition: se3_curve.h:142
Definition of a cubic spline.
interface for a Curve of arbitrary dimension.
SE3Curve< Time, Numeric, Safe > SE3Curve_t
Definition: se3_curve.h:40
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equal given a precision treshold Only two curves ...
Definition: se3_curve.h:167
Eigen::Vector3d point3_t
Definition: fwd.h:58
virtual bool operator==(const SE3Curve_t &other) const
Definition: se3_curve.h:176
boost::shared_ptr< curve_rotation_t > curve_rotation_ptr_t
Definition: se3_curve.h:36
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: se3_curve.h:38
Eigen::Quaternion< Scalar > Quaternion
Definition: se3_curve.h:28
Represents a polynomial of an arbitrary order defined on the interval . It follows the equation : ...
Definition: fwd.h:37
SE3Curve(const pointX_t &init_pos, const pointX_t &end_pos, const Quaternion &init_rot, const Quaternion &end_rot, const time_t &t_min, const time_t &t_max)
Constructor from init/end pose, with quaternion. use polynomial of degree 1 for position and SO3Linea...
Definition: se3_curve.h:70
virtual bool operator!=(const SE3Curve_t &other) const
Definition: se3_curve.h:178
polynomial< Time, Numeric, Safe, pointX_t > polynomial_t
Definition: se3_curve.h:39
curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: se3_curve.h:201
curve_ptr_t translation_curve_
Definition: se3_curve.h:224
double Time
Definition: effector_spline.h:27
SE3Curve()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: se3_curve.h:46
curve_rotation_ptr_t rotation_curve_
Definition: se3_curve.h:225
void serialize(Archive &ar, const unsigned int version)
Definition: se3_curve.h:233
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:65
curve_abc< Time, Numeric, Safe, matrix3_t, point3_t > curve_rotation_t
Definition: se3_curve.h:34
bool isApprox(const SE3Curve_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: se3_curve.h:160
Composition of a curve of any type of dimension 3 and a curve representing an rotation (in current im...
Definition: fwd.h:40
SE3Curve(curve_ptr_t translation_curve, const matrix3_t &init_rot, const matrix3_t &end_rot)
Constructor from curve for the translation and init/end rotation, with rotation matrix. Use SO3Linear for rotation with the same time bounds as the.
Definition: se3_curve.h:108
Eigen::Matrix< Scalar, 6, 1 > point_derivate_t
Definition: se3_curve.h:27
Time time_t
Definition: se3_curve.h:29
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative of order N of spline at time t.
Definition: se3_curve.h:184
time_t T_min_
Definition: se3_curve.h:226
double Numeric
Definition: effector_spline.h:26
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: se3_curve.h:30
Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion.
Definition: fwd.h:46
virtual std::size_t dim() const
Get dimension of curve.
Definition: se3_curve.h:206
const curve_rotation_ptr_t rotation_curve() const
const accessor to the rotation curve
Definition: se3_curve.h:219
curve_derivate_t compute_derivate(const std::size_t) const
Definition: se3_curve.h:194
boost::shared_ptr< curve_X_t > curve_ptr_t
Definition: se3_curve.h:35
polynomial< Time, Numeric, Safe, point_derivate_t > curve_derivate_t
Definition: se3_curve.h:31
SE3Curve(curve_ptr_t translation_curve, const Quaternion &init_rot, const Quaternion &end_rot)
Constructor from curve for the translation and init/end rotation, with quaternion. Use SO3Linear for rotation with the same time bounds as the.
Definition: se3_curve.h:97
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
friend class boost::serialization::access
Definition: se3_curve.h:230