se3_curve.h 11.3 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
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
35
36
  typedef curve_abc<Time, Numeric, Safe, matrix3_t, point3_t>
      curve_rotation_t;  // templated class used for the rotation (return dimension are fixed)
37
38
39
  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
40
41
42
43
44
45
46
  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
47
  ///
48
  SE3Curve() : curve_abc_t(), dim_(3), translation_curve_(), rotation_curve_(), T_min_(0), T_max_(0) {}
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
49
50
51
52
53
54
55
56
57
58
59

  /// \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)
60
61
      : curve_abc_t(),
        dim_(6),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
62
63
64
65
66
67
68
69
70
71
72
73
        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
74
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
75
76
77
78
79
80
81
82
83
84
85
86
        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
87
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
88
89
90
91
92
93
94
95
96
97
98
        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
99
  SE3Curve(curve_ptr_t translation_curve, const Quaternion& init_rot, const Quaternion& end_rot)
Pierre Fernbach's avatar
Pierre Fernbach committed
100
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
101
102
103
104
105
106
107
108
109
        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
110
  SE3Curve(curve_ptr_t translation_curve, const matrix3_t& init_rot, const matrix3_t& end_rot)
Pierre Fernbach's avatar
Pierre Fernbach committed
111
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
112
113
114
115
116
117
118
119
120
121
        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
122
  SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)
Pierre Fernbach's avatar
Pierre Fernbach committed
123
      : curve_abc_t(),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
124
125
        dim_(6),
        translation_curve_(translation_curve),
Pierre Fernbach's avatar
Pierre Fernbach committed
126
        rotation_curve_(rotation_curve),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
127
128
129
130
        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
131
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
132
133
    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
134
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
135
136
    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
137
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
138
139
140
141
142
143
144
145
146
    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");
147
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
148
149
150
151
152
153
    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
154
  /**
155
   * @brief isApprox check if other and *this are approximately equals.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
156
157
   * 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
158
159
160
161
   * @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
   */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
162
163
164
165
166
  bool isApprox(const SE3Curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
    return curves::isApprox<Numeric>(T_min_, other.min()) && curves::isApprox<Numeric>(T_max_, other.max()) &&
           (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
167
168
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
169
170
  virtual bool isApprox(const curve_abc_t* other,
                        const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
171
    const SE3Curve_t* other_cast = dynamic_cast<const SE3Curve_t*>(other);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
172
173
    if (other_cast)
      return isApprox(*other_cast, prec);
174
175
176
177
    else
      return false;
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
178
  virtual bool operator==(const SE3Curve_t& other) const { return isApprox(other); }
179

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
180
  virtual bool operator!=(const SE3Curve_t& other) const { return !(*this == other); }
Pierre Fernbach's avatar
Pierre Fernbach committed
181

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
182
183
184
185
186
187
188
189
190
191
192
193
194
195
  ///  \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;
  }

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

200
201
202
  ///  \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.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
203
  SE3Curve_t* compute_derivate_ptr(const std::size_t order) const { return new SE3Curve_t(compute_derivate(order)); }
204

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
205
206
207
208
209
210
211
212
213
214
  /*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_; }
215
216
  /// \brief Get the degree of the curve.
  /// \return \f$degree\f$, the degree of the curve.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
217
  virtual std::size_t degree() const { return translation_curve_->degree(); }
218
  /// \brief const accessor to the translation curve
219
  const curve_ptr_t translation_curve() const {return translation_curve_;}
220
  /// \brief const accessor to the rotation curve
221
  const curve_rotation_ptr_t rotation_curve() const {return rotation_curve_;}
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
222
223
224
225
  /*Helpers*/

  /*Attributes*/
  std::size_t dim_;  // dim doesn't mean anything in this class ...
226
227
  curve_ptr_t translation_curve_;
  curve_rotation_ptr_t rotation_curve_;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
228
229
230
231
232
233
234
235
236
237
238
  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 ?
    }
239
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
    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
255
256
      }
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
257
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
258

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

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

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