se3_curve.h 10.9 KB
Newer Older
Pierre Fernbach's avatar
Pierre Fernbach committed
1
2
3
4
5
6
7
8
9
10
#ifndef _STRUCT_SE3_CURVE_H
#define _STRUCT_SE3_CURVE_H

#include "MathDefs.h"
#include "curve_abc.h"
#include "so3_linear.h"
#include "polynomial.h"
#include <boost/math/constants/constants.hpp>
#include <Eigen/Dense>

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
namespace curves {

/// \class SE3Curve.
/// \brief Composition of a curve of any type of dimension 3 and a curve representing an rotation
/// (in current implementation, only SO3Linear can be used for the rotation part)
/// The output is a vector of size 7 (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w)
/// The output of the derivative of any order is a vector of size 6
/// (linear_x,linear_y,linear_z,angular_x,angular_y,angular_z)
///
///
template <typename Time = double, typename Numeric = Time, bool Safe = false>
struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric, 3, Eigen::Affine>,
                                   Eigen::Matrix<Numeric, 6, 1> > {
  typedef Numeric Scalar;
  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
  typedef curve_abc<Time, Numeric, Safe, pointX_t> curve_X_t;                     // generic class of curve
35
36
37
38
  typedef curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> curve_rotation_t;  // templated class used for the rotation (return dimension are fixed)
  typedef boost::shared_ptr<curve_X_t> curve_ptr_t;
  typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
39
40
41
42
43
44
45
  typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
  typedef polynomial<Time, Numeric, Safe, pointX_t> polynomial_t;
  typedef SE3Curve<Time, Numeric, Safe> SE3Curve_t;

 public:
  /* Constructors - destructors */
  /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
Pierre Fernbach's avatar
Pierre Fernbach committed
46
  ///
47
  SE3Curve() : curve_abc_t(), dim_(3), translation_curve_(), rotation_curve_(), T_min_(0), T_max_(0) {}
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
48
49
50
51
52
53
54
55
56
57
58

  /// \brief Destructor
  ~SE3Curve() {
    // should we delete translation_curve and rotation_curve here ?
    // better switch to shared ptr
  }

  /* Constructor without curve object for the translation : */
  /// \brief Constructor from init/end transform use polynomial of degree 1 for position and SO3Linear for rotation
  SE3Curve(const transform_t& init_transform, const transform_t& end_transform, const time_t& t_min,
           const time_t& t_max)
59
60
      : curve_abc_t(),
        dim_(6),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
61
62
63
64
65
66
67
68
69
70
71
72
        translation_curve_(new polynomial_t(pointX_t(init_transform.translation()),
                                            pointX_t(end_transform.translation()), t_min, t_max)),
        rotation_curve_(new SO3Linear_t(init_transform.rotation(), end_transform.rotation(), t_min, t_max)),
        T_min_(t_min),
        T_max_(t_max) {
    safe_check();
  }

  /// \brief Constructor from init/end pose, with quaternion. use polynomial of degree 1 for position and SO3Linear for
  /// rotation
  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)
Pierre Fernbach's avatar
Pierre Fernbach committed
73
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
74
75
76
77
78
79
80
81
82
83
84
85
        dim_(6),
        translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)),
        rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
        T_min_(t_min),
        T_max_(t_max) {
    safe_check();
  }

  /// \brief Constructor from init/end pose, with rotation matrix. use polynomial of degree 1 for position and
  /// SO3Linear for rotation
  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)
Pierre Fernbach's avatar
Pierre Fernbach committed
86
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
87
88
89
90
91
92
93
94
95
96
97
        dim_(6),
        translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)),
        rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
        T_min_(t_min),
        T_max_(t_max) {
    safe_check();
  }

  /* Constructor with curve object for the translation : */
  /// \brief Constructor from curve for the translation and init/end rotation, with quaternion.
  /// Use SO3Linear for rotation with the same time bounds as the
98
  SE3Curve(curve_ptr_t translation_curve, const Quaternion& init_rot, const Quaternion& end_rot)
Pierre Fernbach's avatar
Pierre Fernbach committed
99
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
100
101
102
103
104
105
106
107
108
        dim_(6),
        translation_curve_(translation_curve),
        rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())),
        T_min_(translation_curve->min()),
        T_max_(translation_curve->max()) {
    safe_check();
  }
  /// \brief Constructor from curve for the translation and init/end rotation, with rotation matrix.
  /// Use SO3Linear for rotation with the same time bounds as the
109
  SE3Curve(curve_ptr_t translation_curve, const matrix3_t& init_rot, const matrix3_t& end_rot)
Pierre Fernbach's avatar
Pierre Fernbach committed
110
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
111
112
113
114
115
116
117
118
119
120
        dim_(6),
        translation_curve_(translation_curve),
        rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())),
        T_min_(translation_curve->min()),
        T_max_(translation_curve->max()) {
    safe_check();
  }

  /* Constructor from translation and rotation curves object : */
  /// \brief Constructor from from translation and rotation curves object
121
  SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)
Pierre Fernbach's avatar
Pierre Fernbach committed
122
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
123
124
        dim_(6),
        translation_curve_(translation_curve),
Pierre Fernbach's avatar
Pierre Fernbach committed
125
        rotation_curve_(rotation_curve),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
126
127
128
129
        T_min_(translation_curve->min()),
        T_max_(translation_curve->max()) {
    if (translation_curve->dim() != 3) {
      throw std::invalid_argument("The translation curve should be of dimension 3.");
Pierre Fernbach's avatar
Pierre Fernbach committed
130
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
131
132
    if (rotation_curve->min() != T_min_) {
      throw std::invalid_argument("Min bounds of translation and rotation curve are not the same.");
Pierre Fernbach's avatar
Pierre Fernbach committed
133
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
134
135
    if (rotation_curve->max() != T_max_) {
      throw std::invalid_argument("Max bounds of translation and rotation curve are not the same.");
Pierre Fernbach's avatar
Pierre Fernbach committed
136
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
137
138
139
140
141
142
143
144
145
    safe_check();
  }

  ///  \brief Evaluation of the SE3Curve at time t
  ///  \param t : time when to evaluate the spline.
  ///  \return \f$x(t)\f$ point corresponding on spline at time t. (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w)
  virtual point_t operator()(const time_t t) const {
    if (translation_curve_->dim() != 3) {
      throw std::invalid_argument("Translation curve should always be of dimension 3");
146
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
147
148
149
150
151
152
    point_t res = point_t::Identity();
    res.translate(point3_t((*translation_curve_)(t)));
    res.rotate((*rotation_curve_)(t));
    return res;
  }

Pierre Fernbach's avatar
Pierre Fernbach committed
153
  /**
154
155
   * @brief isApprox check if other and *this are approximately equals.
   * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
Pierre Fernbach's avatar
Pierre Fernbach committed
156
157
158
159
   * @param other the other curve to check
   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
   * @return true is the two curves are approximately equals
   */
160
  bool isApprox(const SE3Curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
Pierre Fernbach's avatar
Pierre Fernbach committed
161
162
    return T_min_ == other.min()
        && T_max_ == other.max()
163
164
        && (translation_curve_ == other.translation_curve_ || translation_curve_->isApprox(other.translation_curve_.get(),prec))
        && (rotation_curve_ == other.rotation_curve_ || rotation_curve_->isApprox(other.rotation_curve_.get(),prec));
Pierre Fernbach's avatar
Pierre Fernbach committed
165
166
  }

167
168
169
170
171
172
173
174
175
  virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
    const SE3Curve_t* other_cast = dynamic_cast<const SE3Curve_t*>(other);
    if(other_cast)
      return isApprox(*other_cast,prec);
    else
      return false;
  }


Pierre Fernbach's avatar
Pierre Fernbach committed
176
177
178
179
180
181
182
183
184
  virtual bool operator==(const SE3Curve_t& other) const {
    return isApprox(other);
  }

  virtual bool operator!=(const SE3Curve_t& other) const {
    return !(*this == other);
  }


Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
185
186
187
188
189
190
191
192
193
194
195
196
197
198
  ///  \brief Evaluation of the derivative of order N of spline at time t.
  ///  \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_derivate_t derivate(const time_t t, const std::size_t order) const {
    if (translation_curve_->dim() != 3) {
      throw std::invalid_argument("Translation curve should always be of dimension 3");
    }
    point_derivate_t res = point_derivate_t::Zero();
    res.segment(0, 3) = point3_t(translation_curve_->derivate(t, order));
    res.segment(3, 3) = rotation_curve_->derivate(t, order);
    return res;
  }

199
  SE3Curve_t compute_derivate(const std::size_t /*order*/) const {
200
201
202
    throw std::logic_error("Compute derivate for SE3 is not implemented yet.");
  }

203
204
205
206
207
208
209
  ///  \brief Compute the derived curve at order N.
  ///  \param order : order of derivative.
  ///  \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
  SE3Curve_t* compute_derivate_ptr(const std::size_t order) const {
    return new SE3Curve_t(compute_derivate(order));
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
210
211
212
213
214
215
216
217
218
219
  /*Helpers*/
  /// \brief Get dimension of curve.
  /// \return dimension of curve.
  std::size_t virtual dim() const { return dim_; };
  /// \brief Get the minimum time for which the curve is defined
  /// \return \f$t_{min}\f$ lower bound of time range.
  time_t min() const { return T_min_; }
  /// \brief Get the maximum time for which the curve is defined.
  /// \return \f$t_{max}\f$ upper bound of time range.
  time_t max() const { return T_max_; }
220
221
222
  /// \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();}
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
223
224
225
226
  /*Helpers*/

  /*Attributes*/
  std::size_t dim_;  // dim doesn't mean anything in this class ...
227
228
  curve_ptr_t translation_curve_;
  curve_rotation_ptr_t rotation_curve_;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
229
230
231
232
233
234
235
236
237
238
239
  time_t T_min_, T_max_;
  /*Attributes*/

  // Serialization of the class
  friend class boost::serialization::access;

  template <class Archive>
  void serialize(Archive& ar, const unsigned int version) {
    if (version) {
      // Do something depending on version ?
    }
240
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
    ar& boost::serialization::make_nvp("dim", dim_);
    ar& boost::serialization::make_nvp("translation_curve", translation_curve_);
    ar& boost::serialization::make_nvp("rotation_curve", rotation_curve_);
    ar& boost::serialization::make_nvp("T_min", T_min_);
    ar& boost::serialization::make_nvp("T_max", T_max_);
  }

 private:
  void safe_check() {
    if (Safe) {
      if (T_min_ > T_max_) {
        throw std::invalid_argument("Tmin should be inferior to Tmax");
      }
      if (translation_curve_->dim() != 3) {
        throw std::invalid_argument("Translation curve should always be of dimension 3");
Pierre Fernbach's avatar
Pierre Fernbach committed
256
257
      }
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
258
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
259

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
260
};  // SE3Curve
Pierre Fernbach's avatar
Pierre Fernbach committed
261

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
262
}  // namespace curves
Pierre Fernbach's avatar
Pierre Fernbach committed
263

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
264
#endif  // SE3_CURVE_H