Loading...
Searching...
No Matches
se3_curve.h
Go to the documentation of this file.
1#ifndef _STRUCT_SE3_CURVE_H
2#define _STRUCT_SE3_CURVE_H
3
4#include <Eigen/Dense>
5#include <boost/math/constants/constants.hpp>
6
7#include "MathDefs.h"
8#include "curve_abc.h"
9#include "polynomial.h"
10#include "so3_linear.h"
11
12namespace ndcurves {
13
23template <typename Time = double, typename Numeric = Time, bool Safe = false>
24struct SE3Curve : public curve_abc<Time, Numeric, Safe,
25 Eigen::Transform<Numeric, 3, Eigen::Affine>,
26 Eigen::Matrix<Numeric, 6, 1> > {
27 typedef Numeric Scalar;
28 typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
30 typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t;
31 typedef Eigen::Quaternion<Scalar> Quaternion;
32 typedef Time time_t;
34 curve_abc_t; // parent class
37 curve_X_t; // generic class of curve
39 curve_rotation_t; // templated class used for the rotation (return
40 // dimension are fixed)
41 typedef boost::shared_ptr<curve_X_t> curve_ptr_t;
42 typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
43
47
48 public:
49 /* Constructors - destructors */
54 : curve_abc_t(),
55 dim_(3),
58 T_min_(0),
59 T_max_(0) {}
60
62 virtual ~SE3Curve() {
63 // should we delete translation_curve and rotation_curve here ?
64 // better switch to shared ptr
65 }
66
67 /* Constructor without curve object for the translation : */
70 SE3Curve(const transform_t& init_transform, const transform_t& end_transform,
71 const time_t& t_min, const time_t& t_max)
72 : curve_abc_t(),
73 dim_(6),
75 pointX_t(init_transform.translation()),
76 pointX_t(end_transform.translation()), t_min, t_max)),
78 init_transform.rotation(), end_transform.rotation(), t_min, t_max)),
79 T_min_(t_min),
80 T_max_(t_max) {
81 safe_check();
82 }
83
86 SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos,
87 const Quaternion& init_rot, const Quaternion& end_rot,
88 const time_t& t_min, const time_t& t_max)
89 : curve_abc_t(),
90 dim_(6),
91 translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)),
92 rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
93 T_min_(t_min),
94 T_max_(t_max) {
95 safe_check();
96 }
97
100 SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos,
101 const matrix3_t& init_rot, const matrix3_t& end_rot,
102 const time_t& t_min, const time_t& t_max)
103 : curve_abc_t(),
104 dim_(6),
105 translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)),
106 rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
107 T_min_(t_min),
108 T_max_(t_max) {
109 safe_check();
110 }
111
112 /* Constructor with curve object for the translation : */
117 const Quaternion& end_rot)
118 : curve_abc_t(),
119 dim_(6),
121 rotation_curve_(new SO3Linear_t(init_rot, end_rot,
126 safe_check();
127 }
132 const matrix3_t& end_rot)
133 : curve_abc_t(),
134 dim_(6),
136 rotation_curve_(new SO3Linear_t(init_rot, end_rot,
141 safe_check();
142 }
143
144 /* Constructor from translation and rotation curves object : */
147 : curve_abc_t(),
148 dim_(6),
153 if (translation_curve->dim() != 3) {
154 throw std::invalid_argument(
155 "The translation curve should be of dimension 3.");
156 }
157 if (rotation_curve->min() != T_min_) {
158 throw std::invalid_argument(
159 "Min bounds of translation and rotation curve are not the same.");
160 }
161 if (rotation_curve->max() != T_max_) {
162 throw std::invalid_argument(
163 "Max bounds of translation and rotation curve are not the same.");
164 }
165 safe_check();
166 }
167
172 virtual point_t operator()(const time_t t) const {
173 if (translation_curve_->dim() != 3) {
174 throw std::invalid_argument(
175 "Translation curve should always be of dimension 3");
176 }
177 point_t res = point_t::Identity();
178 res.translate(point3_t((*translation_curve_)(t)));
179 res.rotate((*rotation_curve_)(t));
180 return res;
181 }
182
193 const SE3Curve_t& other,
194 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
195 return ndcurves::isApprox<Numeric>(T_min_, other.min()) &&
196 ndcurves::isApprox<Numeric>(T_max_, other.max()) &&
198 translation_curve_->isApprox(other.translation_curve_.get(),
199 prec)) &&
201 rotation_curve_->isApprox(other.rotation_curve_.get(), prec));
202 }
203
204 virtual bool isApprox(
205 const curve_abc_t* other,
206 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
207 const SE3Curve_t* other_cast = dynamic_cast<const SE3Curve_t*>(other);
208 if (other_cast)
209 return isApprox(*other_cast, prec);
210 else
211 return false;
212 }
213
214 virtual bool operator==(const SE3Curve_t& other) const {
215 return isApprox(other);
216 }
217
218 virtual bool operator!=(const SE3Curve_t& other) const {
219 return !(*this == other);
220 }
221
228 const std::size_t order) const {
229 if (translation_curve_->dim() != 3) {
230 throw std::invalid_argument(
231 "Translation curve should always be of dimension 3");
232 }
233 point_derivate_t res = point_derivate_t::Zero();
234 res.segment(0, 3) = point3_t(translation_curve_->derivate(t, order));
235 res.segment(3, 3) = rotation_curve_->derivate(t, order);
236 return res;
237 }
238
239 curve_derivate_t compute_derivate(const std::size_t /*order*/) const {
240 throw std::logic_error("Compute derivate for SE3 is not implemented yet.");
241 }
242
247 curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
248 return new curve_derivate_t(compute_derivate(order));
249 }
250
251 /*Helpers*/
254 std::size_t virtual dim() const { return dim_; };
257 time_t min() const { return T_min_; }
260 time_t max() const { return T_max_; }
263 virtual std::size_t degree() const { return translation_curve_->degree(); }
268 /*Helpers*/
269
270 /*Attributes*/
271 std::size_t dim_; // dim doesn't mean anything in this class ...
275 /*Attributes*/
276
277 // Serialization of the class
279
280 template <class Archive>
281 void serialize(Archive& ar, const unsigned int version) {
282 if (version) {
283 // Do something depending on version ?
284 }
285 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
286 ar& boost::serialization::make_nvp("dim", dim_);
287 ar& boost::serialization::make_nvp("translation_curve", translation_curve_);
288 ar& boost::serialization::make_nvp("rotation_curve", rotation_curve_);
289 ar& boost::serialization::make_nvp("T_min", T_min_);
290 ar& boost::serialization::make_nvp("T_max", T_max_);
291 }
292
293 private:
294 void safe_check() {
295 if (Safe) {
296 if (T_min_ > T_max_) {
297 throw std::invalid_argument("Tmin should be inferior to Tmax");
298 }
299 if (translation_curve_->dim() != 3) {
300 throw std::invalid_argument(
301 "Translation curve should always be of dimension 3");
302 }
303 }
304 }
305
306}; // SE3Curve
307
308} // namespace ndcurves
309
311 SINGLE_ARG(typename Time, typename Numeric, bool Safe),
313
314#endif // SE3_CURVE_H
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
#define SINGLE_ARG(...)
Definition: archive.hpp:23
interface for a Curve of arbitrary dimension.
Definition: bernstein.h:20
Eigen::Vector3d point3_t
Definition: fwd.h:67
Eigen::VectorXd pointX_t
Definition: fwd.h:69
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:70
Definition of a cubic spline.
Composition of a curve of any type of dimension 3 and a curve representing an rotation (in current im...
Definition: se3_curve.h:26
SE3Curve()
Empty constructor. Curve obtained this way can not perform other class functions.
Definition: se3_curve.h:53
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....
Definition: se3_curve.h:131
Eigen::Matrix< Scalar, 6, 1 > point_derivate_t
Definition: se3_curve.h:30
time_t max() const
Get the maximum time for which the curve is defined.
Definition: se3_curve.h:260
SE3Curve< Time, Numeric, Safe > SE3Curve_t
Definition: se3_curve.h:46
Eigen::Transform< Numeric, 3, Eigen::Affine > transform_t
Definition: se3_curve.h:28
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:70
curve_abc< Time, Numeric, Safe, pointX_t > curve_X_t
Definition: se3_curve.h:37
virtual point_t operator()(const time_t t) const
Evaluation of the SE3Curve at time t.
Definition: se3_curve.h:172
virtual std::size_t dim() const
Get dimension of curve.
Definition: se3_curve.h:254
SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)
Constructor from from translation and rotation curves object.
Definition: se3_curve.h:146
polynomial< Time, Numeric, Safe, point_derivate_t > curve_derivate_t
Definition: se3_curve.h:35
curve_derivate_t compute_derivate(const std::size_t) const
Definition: se3_curve.h:239
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: se3_curve.h:44
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:227
virtual bool operator!=(const SE3Curve_t &other) const
Definition: se3_curve.h:218
curve_ptr_t translation_curve_
Definition: se3_curve.h:272
transform_t point_t
Definition: se3_curve.h:29
Time time_t
Definition: se3_curve.h:32
time_t min() const
Get the minimum time for which the curve is defined.
Definition: se3_curve.h:257
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:204
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: se3_curve.h:34
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:100
virtual ~SE3Curve()
Destructor.
Definition: se3_curve.h:62
boost::shared_ptr< curve_rotation_t > curve_rotation_ptr_t
Definition: se3_curve.h:42
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....
Definition: se3_curve.h:116
const curve_rotation_ptr_t rotation_curve() const
const accessor to the rotation curve
Definition: se3_curve.h:267
boost::shared_ptr< curve_X_t > curve_ptr_t
Definition: se3_curve.h:41
time_t T_min_
Definition: se3_curve.h:274
Eigen::Quaternion< Scalar > Quaternion
Definition: se3_curve.h:31
friend class boost::serialization::access
Definition: se3_curve.h:278
std::size_t dim_
Definition: se3_curve.h:271
curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: se3_curve.h:247
polynomial< Time, Numeric, Safe, pointX_t > polynomial_t
Definition: se3_curve.h:45
void serialize(Archive &ar, const unsigned int version)
Definition: se3_curve.h:281
virtual std::size_t degree() const
Get the degree of the curve.
Definition: se3_curve.h:263
const curve_ptr_t translation_curve() const
const accessor to the translation curve
Definition: se3_curve.h:265
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:192
Numeric Scalar
Definition: se3_curve.h:27
virtual bool operator==(const SE3Curve_t &other) const
Definition: se3_curve.h:214
curve_rotation_ptr_t rotation_curve_
Definition: se3_curve.h:273
time_t T_max_
Definition: se3_curve.h:274
curve_abc< Time, Numeric, Safe, matrix3_t, point3_t > curve_rotation_t
Definition: se3_curve.h:39
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:86
Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion.
Definition: so3_linear.h:21
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37
Represents a polynomial of an arbitrary order defined on the interval . It follows the equation : ...
Definition: polynomial.h:35