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 "MathDefs.h"
5 #include "curve_abc.h"
6 #include "so3_linear.h"
7 #include "polynomial.h"
8 #include <boost/math/constants/constants.hpp>
9 #include <Eigen/Dense>
10 
11 namespace ndcurves {
12 
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> > {
24  typedef Numeric Scalar;
25  typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
26  typedef transform_t point_t;
27  typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t;
28  typedef Eigen::Quaternion<Scalar> Quaternion;
29  typedef Time time_t;
32  typedef curve_abc<Time, Numeric, Safe, pointX_t> curve_X_t; // generic class of curve
34  curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
35  typedef boost::shared_ptr<curve_X_t> curve_ptr_t;
36  typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
37 
41 
42  public:
43  /* Constructors - destructors */
46  SE3Curve() : curve_abc_t(), dim_(3), translation_curve_(), rotation_curve_(), T_min_(0), T_max_(0) {}
47 
49  virtual ~SE3Curve() {
50  // should we delete translation_curve and rotation_curve here ?
51  // better switch to shared ptr
52  }
53 
54  /* Constructor without curve object for the translation : */
56  SE3Curve(const transform_t& init_transform, const transform_t& end_transform, const time_t& t_min,
57  const time_t& t_max)
58  : curve_abc_t(),
59  dim_(6),
60  translation_curve_(new polynomial_t(pointX_t(init_transform.translation()),
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)),
63  T_min_(t_min),
64  T_max_(t_max) {
65  safe_check();
66  }
67 
70  SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const Quaternion& init_rot, const Quaternion& end_rot,
71  const time_t& t_min, const time_t& t_max)
72  : curve_abc_t(),
73  dim_(6),
74  translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)),
75  rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
76  T_min_(t_min),
77  T_max_(t_max) {
78  safe_check();
79  }
80 
83  SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const matrix3_t& init_rot, const matrix3_t& end_rot,
84  const time_t& t_min, const time_t& t_max)
85  : curve_abc_t(),
86  dim_(6),
87  translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)),
88  rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
89  T_min_(t_min),
90  T_max_(t_max) {
91  safe_check();
92  }
93 
94  /* Constructor with curve object for the translation : */
97  SE3Curve(curve_ptr_t translation_curve, const Quaternion& init_rot, const Quaternion& end_rot)
98  : curve_abc_t(),
99  dim_(6),
100  translation_curve_(translation_curve),
101  rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())),
102  T_min_(translation_curve->min()),
103  T_max_(translation_curve->max()) {
104  safe_check();
105  }
108  SE3Curve(curve_ptr_t translation_curve, const matrix3_t& init_rot, const matrix3_t& end_rot)
109  : curve_abc_t(),
110  dim_(6),
111  translation_curve_(translation_curve),
112  rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())),
113  T_min_(translation_curve->min()),
114  T_max_(translation_curve->max()) {
115  safe_check();
116  }
117 
118  /* Constructor from translation and rotation curves object : */
120  SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)
121  : curve_abc_t(),
122  dim_(6),
123  translation_curve_(translation_curve),
124  rotation_curve_(rotation_curve),
125  T_min_(translation_curve->min()),
126  T_max_(translation_curve->max()) {
127  if (translation_curve->dim() != 3) {
128  throw std::invalid_argument("The translation curve should be of dimension 3.");
129  }
130  if (rotation_curve->min() != T_min_) {
131  throw std::invalid_argument("Min bounds of translation and rotation curve are not the same.");
132  }
133  if (rotation_curve->max() != T_max_) {
134  throw std::invalid_argument("Max bounds of translation and rotation curve are not the same.");
135  }
136  safe_check();
137  }
138 
142  virtual point_t operator()(const time_t t) const {
143  if (translation_curve_->dim() != 3) {
144  throw std::invalid_argument("Translation curve should always be of dimension 3");
145  }
146  point_t res = point_t::Identity();
147  res.translate(point3_t((*translation_curve_)(t)));
148  res.rotate((*rotation_curve_)(t));
149  return res;
150  }
151 
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()) &&
163  translation_curve_->isApprox(other.translation_curve_.get(), prec)) &&
164  (rotation_curve_ == other.rotation_curve_ || rotation_curve_->isApprox(other.rotation_curve_.get(), prec));
165  }
166 
167  virtual bool isApprox(const curve_abc_t* other,
168  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
169  const SE3Curve_t* other_cast = dynamic_cast<const SE3Curve_t*>(other);
170  if (other_cast)
171  return isApprox(*other_cast, prec);
172  else
173  return false;
174  }
175 
176  virtual bool operator==(const SE3Curve_t& other) const { return isApprox(other); }
177 
178  virtual bool operator!=(const SE3Curve_t& other) const { return !(*this == other); }
179 
184  virtual point_derivate_t derivate(const time_t t, const std::size_t order) const {
185  if (translation_curve_->dim() != 3) {
186  throw std::invalid_argument("Translation curve should always be of dimension 3");
187  }
188  point_derivate_t res = point_derivate_t::Zero();
189  res.segment(0, 3) = point3_t(translation_curve_->derivate(t, order));
190  res.segment(3, 3) = rotation_curve_->derivate(t, order);
191  return res;
192  }
193 
194  curve_derivate_t compute_derivate(const std::size_t /*order*/) const {
195  throw std::logic_error("Compute derivate for SE3 is not implemented yet.");
196  }
197 
201  curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
202  return new curve_derivate_t(compute_derivate(order));
203  }
204 
205  /*Helpers*/
208  std::size_t virtual dim() const { return dim_; };
211  time_t min() const { return T_min_; }
214  time_t max() const { return T_max_; }
217  virtual std::size_t degree() const { return translation_curve_->degree(); }
219  const curve_ptr_t translation_curve() const { return translation_curve_; }
221  const curve_rotation_ptr_t rotation_curve() const { return rotation_curve_; }
222  /*Helpers*/
223 
224  /*Attributes*/
225  std::size_t dim_; // dim doesn't mean anything in this class ...
226  curve_ptr_t translation_curve_;
227  curve_rotation_ptr_t rotation_curve_;
228  time_t T_min_, T_max_;
229  /*Attributes*/
230 
231  // Serialization of the class
233 
234  template <class Archive>
235  void serialize(Archive& ar, const unsigned int version) {
236  if (version) {
237  // Do something depending on version ?
238  }
239  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
240  ar& boost::serialization::make_nvp("dim", dim_);
241  ar& boost::serialization::make_nvp("translation_curve", translation_curve_);
242  ar& boost::serialization::make_nvp("rotation_curve", rotation_curve_);
243  ar& boost::serialization::make_nvp("T_min", T_min_);
244  ar& boost::serialization::make_nvp("T_max", T_max_);
245  }
246 
247  private:
248  void safe_check() {
249  if (Safe) {
250  if (T_min_ > T_max_) {
251  throw std::invalid_argument("Tmin should be inferior to Tmax");
252  }
253  if (translation_curve_->dim() != 3) {
254  throw std::invalid_argument("Translation curve should always be of dimension 3");
255  }
256  }
257  }
258 
259 }; // SE3Curve
260 
261 } // namespace ndcurves
262 
263 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe),
265 
266 #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:219
std::size_t dim_
Definition: se3_curve.h:225
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:214
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:228
virtual ~SE3Curve()
Destructor.
Definition: se3_curve.h:49
virtual std::size_t degree() const
Get the degree of the curve.
Definition: se3_curve.h:217
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:211
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
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:226
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:227
void serialize(Archive &ar, const unsigned int version)
Definition: se3_curve.h:235
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:228
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:208
const curve_rotation_ptr_t rotation_curve() const
const accessor to the rotation curve
Definition: se3_curve.h:221
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:232