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 
12 namespace ndcurves {
13 
23 template <typename Time = double, typename Numeric = Time, bool Safe = false>
24 struct 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_translation_t; // templated class used for the translation (return
40  // dimension are fixed)
42  curve_rotation_t; // templated class used for the rotation (return
43  // dimension are fixed)
44  typedef boost::shared_ptr<curve_X_t> curve_ptr_t;
45  typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
46  typedef boost::shared_ptr<curve_translation_t> curve_translation_ptr_t;
47 
51 
52  public:
53  /* Constructors - destructors */
58  : curve_abc_t(),
59  dim_(3),
62  T_min_(0),
63  T_max_(0) {}
64 
66  virtual ~SE3Curve() {
67  // should we delete translation_curve and rotation_curve here ?
68  // better switch to shared ptr
69  }
70 
71  /* Constructor without curve object for the translation : */
74  SE3Curve(const transform_t& init_transform, const transform_t& end_transform,
75  const time_t& t_min, const time_t& t_max)
76  : curve_abc_t(),
77  dim_(6),
79  point3_t(init_transform.translation()),
80  point3_t(end_transform.translation()), t_min, t_max)),
82  init_transform.rotation(), end_transform.rotation(), t_min, t_max)),
83  T_min_(t_min),
84  T_max_(t_max) {
85  safe_check();
86  }
87 
90  SE3Curve(const point3_t& init_pos, const point3_t& end_pos,
91  const Quaternion& init_rot, const Quaternion& end_rot,
92  const time_t& t_min, const time_t& t_max)
93  : curve_abc_t(),
94  dim_(6),
95  translation_curve_(new polynomial3_t(init_pos, end_pos, t_min, t_max)),
96  rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
97  T_min_(t_min),
98  T_max_(t_max) {
99  safe_check();
100  }
101 
104  SE3Curve(const point3_t& init_pos, const point3_t& end_pos,
105  const matrix3_t& init_rot, const matrix3_t& end_rot,
106  const time_t& t_min, const time_t& t_max)
107  : curve_abc_t(),
108  dim_(6),
109  translation_curve_(new polynomial3_t(init_pos, end_pos, t_min, t_max)),
110  rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
111  T_min_(t_min),
112  T_max_(t_max) {
113  safe_check();
114  }
115 
116  /* Constructor with curve object for the translation : */
121  const Quaternion& init_rot, const Quaternion& end_rot)
122  : curve_abc_t(),
123  dim_(6),
125  rotation_curve_(new SO3Linear_t(init_rot, end_rot,
127  translation_curve->max())),
130  safe_check();
131  }
136  const matrix3_t& end_rot)
137  : curve_abc_t(),
138  dim_(6),
140  rotation_curve_(new SO3Linear_t(init_rot, end_rot,
142  translation_curve->max())),
145  safe_check();
146  }
147 
148  /* Constructor from translation and rotation curves object : */
152  : curve_abc_t(),
153  dim_(6),
158  if (translation_curve->dim() != 3) {
159  throw std::invalid_argument(
160  "The translation curve should be of dimension 3.");
161  }
162  if (rotation_curve->min() != T_min_) {
163  throw std::invalid_argument(
164  "Min bounds of translation and rotation curve are not the same.");
165  }
166  if (rotation_curve->max() != T_max_) {
167  throw std::invalid_argument(
168  "Max bounds of translation and rotation curve are not the same.");
169  }
170  safe_check();
171  }
172 
177  virtual point_t operator()(const time_t t) const {
178  if (translation_curve_->dim() != 3) {
179  throw std::invalid_argument(
180  "Translation curve should always be of dimension 3");
181  }
182  point_t res = point_t::Identity();
183  res.translate(point3_t((*translation_curve_)(t)));
184  res.rotate((*rotation_curve_)(t));
185  return res;
186  }
187 
197  bool isApprox(
198  const SE3Curve_t& other,
199  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
200  return ndcurves::isApprox<Numeric>(T_min_, other.min()) &&
201  ndcurves::isApprox<Numeric>(T_max_, other.max()) &&
203  translation_curve_->isApprox(other.translation_curve_.get(),
204  prec)) &&
205  (rotation_curve_ == other.rotation_curve_ ||
206  rotation_curve_->isApprox(other.rotation_curve_.get(), prec));
207  }
208 
209  virtual bool isApprox(
210  const curve_abc_t* other,
211  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
212  const SE3Curve_t* other_cast = dynamic_cast<const SE3Curve_t*>(other);
213  if (other_cast)
214  return isApprox(*other_cast, prec);
215  else
216  return false;
217  }
218 
219  virtual bool operator==(const SE3Curve_t& other) const {
220  return isApprox(other);
221  }
222 
223  virtual bool operator!=(const SE3Curve_t& other) const {
224  return !(*this == other);
225  }
226 
233  const std::size_t order) const {
234  if (translation_curve_->dim() != 3) {
235  throw std::invalid_argument(
236  "Translation curve should always be of dimension 3");
237  }
238  point_derivate_t res = point_derivate_t::Zero();
239  res.segment(0, 3) = point3_t(translation_curve_->derivate(t, order));
240  res.segment(3, 3) = rotation_curve_->derivate(t, order);
241  return res;
242  }
243 
244  curve_derivate_t compute_derivate(const std::size_t /*order*/) const {
245  throw std::logic_error("Compute derivate for SE3 is not implemented yet.");
246  }
247 
252  curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
253  return new curve_derivate_t(compute_derivate(order));
254  }
255 
256  /*Helpers*/
259  std::size_t virtual dim() const { return dim_; };
262  time_t min() const { return T_min_; }
265  time_t max() const { return T_max_; }
268  virtual std::size_t degree() const { return translation_curve_->degree(); }
271  return translation_curve_;
272  }
275  /*Helpers*/
276 
277  /*Attributes*/
278  std::size_t dim_; // dim doesn't mean anything in this class ...
282  /*Attributes*/
283 
284  // Serialization of the class
286 
287  template <class Archive>
288  void serialize(Archive& ar, const unsigned int version) {
289  if (version) {
290  // Do something depending on version ?
291  }
292  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
293  ar& boost::serialization::make_nvp("dim", dim_);
294  ar& boost::serialization::make_nvp("translation_curve", translation_curve_);
295  ar& boost::serialization::make_nvp("rotation_curve", rotation_curve_);
296  ar& boost::serialization::make_nvp("T_min", T_min_);
297  ar& boost::serialization::make_nvp("T_max", T_max_);
298  }
299 
300  private:
301  void safe_check() {
302  if (Safe) {
303  if (T_min_ > T_max_) {
304  throw std::invalid_argument("Tmin should be inferior to Tmax");
305  }
306  if (translation_curve_->dim() != 3) {
307  throw std::invalid_argument(
308  "Translation curve should always be of dimension 3");
309  }
310  }
311  }
312 
313 }; // SE3Curve
314 
315 } // namespace ndcurves
316 
318  SINGLE_ARG(typename Time, typename Numeric, bool Safe),
320 
321 #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.
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
Definition: bernstein.h:20
Eigen::Vector3d point3_t
Definition: fwd.h:71
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:74
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:57
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:265
const curve_translation_ptr_t translation_curve() const
const accessor to the translation curve
Definition: se3_curve.h:270
SE3Curve< Time, Numeric, Safe > SE3Curve_t
Definition: se3_curve.h:50
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:74
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:177
curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: se3_curve.h:252
virtual std::size_t dim() const
Get dimension of curve.
Definition: se3_curve.h:259
SE3Curve(const point3_t &init_pos, const point3_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:90
polynomial< Time, Numeric, Safe, point_derivate_t > curve_derivate_t
Definition: se3_curve.h:35
curve_translation_ptr_t translation_curve_
Definition: se3_curve.h:279
curve_derivate_t compute_derivate(const std::size_t) const
Definition: se3_curve.h:244
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: se3_curve.h:48
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:232
virtual bool operator!=(const SE3Curve_t &other) const
Definition: se3_curve.h:223
SE3Curve(curve_translation_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)
Constructor from from translation and rotation curves object.
Definition: se3_curve.h:150
transform_t point_t
Definition: se3_curve.h:29
boost::shared_ptr< curve_translation_t > curve_translation_ptr_t
Definition: se3_curve.h:46
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:262
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: se3_curve.h:209
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: se3_curve.h:34
virtual ~SE3Curve()
Destructor.
Definition: se3_curve.h:66
boost::shared_ptr< curve_rotation_t > curve_rotation_ptr_t
Definition: se3_curve.h:45
curve_abc< Time, Numeric, Safe, point3_t, point3_t > curve_translation_t
Definition: se3_curve.h:39
SE3Curve(curve_translation_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:120
SE3Curve(curve_translation_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:135
const curve_rotation_ptr_t rotation_curve() const
const accessor to the rotation curve
Definition: se3_curve.h:274
boost::shared_ptr< curve_X_t > curve_ptr_t
Definition: se3_curve.h:44
time_t T_min_
Definition: se3_curve.h:281
Eigen::Quaternion< Scalar > Quaternion
Definition: se3_curve.h:31
SE3Curve(const point3_t &init_pos, const point3_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:104
friend class boost::serialization::access
Definition: se3_curve.h:285
std::size_t dim_
Definition: se3_curve.h:278
polynomial< Time, Numeric, Safe, pointX_t > polynomial_t
Definition: se3_curve.h:49
void serialize(Archive &ar, const unsigned int version)
Definition: se3_curve.h:288
virtual std::size_t degree() const
Get the degree of the curve.
Definition: se3_curve.h:268
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:197
Numeric Scalar
Definition: se3_curve.h:27
virtual bool operator==(const SE3Curve_t &other) const
Definition: se3_curve.h:219
curve_rotation_ptr_t rotation_curve_
Definition: se3_curve.h:280
time_t T_max_
Definition: se3_curve.h:281
curve_abc< Time, Numeric, Safe, matrix3_t, point3_t > curve_rotation_t
Definition: se3_curve.h:42
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