so3_linear.h 11.2 KB
Newer Older
Pierre Fernbach's avatar
Pierre Fernbach committed
1
2
3
4
5
6
7
8
9
#ifndef _STRUCT_SO3_LINEAR_H
#define _STRUCT_SO3_LINEAR_H

#include "MathDefs.h"

#include "curve_abc.h"
#include <Eigen/Geometry>
#include <boost/math/constants/constants.hpp>

10
11
12
13
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>


Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
14
15
16
17
18
19
20
21
22
23
24
namespace curves {

/// \class SO3Linear.
/// \brief Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion
///
///
template <typename Time = double, typename Numeric = Time, bool Safe = false>
struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, 3, 3>, Eigen::Matrix<Numeric, 3, 1> > {
  typedef Numeric Scalar;
  typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
  typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
25
26
  typedef matrix3_t point_t;
  typedef point3_t point_derivate_t;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
27
28
  typedef Eigen::Quaternion<Scalar> quaternion_t;
  typedef Time time_t;
29
  typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
30
31
32
33
34
  typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;

 public:
  /* Constructors - destructors */
  /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
Pierre Fernbach's avatar
Pierre Fernbach committed
35
  ///
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
36
37
38
39
  SO3Linear() : curve_abc_t(), dim_(3), init_rot_(), end_rot_(), angular_vel_(), T_min_(0), T_max_(0) {}

  /// \brief constructor with initial and final rotation and time bounds
  SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot, const time_t t_min, const time_t t_max)
40
41
42
43
      : curve_abc_t(), dim_(3), init_rot_(init_rot), end_rot_(end_rot),
        angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix()) / (t_max - t_min)),
        T_min_(t_min), T_max_(t_max)
  {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
44
45
46
47
48
49
50
51
52
    safe_check();
  }

  /// \brief constructor with initial and final rotation expressed as rotation matrix and time bounds
  SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot, const time_t t_min, const time_t t_max)
      : curve_abc_t(),
        dim_(3),
        init_rot_(quaternion_t(init_rot)),
        end_rot_(quaternion_t(end_rot)),
53
        angular_vel_(log3(init_rot.transpose() * end_rot) / (t_max - t_min)),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
54
        T_min_(t_min),
55
56
        T_max_(t_max)
  {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
57
58
59
60
61
    safe_check();
  }

  /// \brief constructor with initial and final rotation, time bounds are set to [0;1]
  SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot)
62
63
64
65
66
      : curve_abc_t(), dim_(3), init_rot_(init_rot), end_rot_(end_rot),
        angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix())),
        T_min_(0.), T_max_(1.)
  {
    safe_check();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
67
68
69
70
71
72
73
74
  }

  /// \brief constructor with initial and final rotation expressed as rotation matrix, time bounds are set to [0;1]
  SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot)
      : curve_abc_t(),
        dim_(3),
        init_rot_(quaternion_t(init_rot)),
        end_rot_(quaternion_t(end_rot)),
75
        angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix())),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
76
        T_min_(0.),
77
78
79
        T_max_(1.)
  {
    safe_check();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
80
81
82
83
  }

  /// \brief Destructor
  ~SO3Linear() {
Pierre Fernbach's avatar
Pierre Fernbach committed
84
    // NOTHING
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
85
86
87
88
89
90
  }

  SO3Linear(const SO3Linear& other)
      : dim_(other.dim_),
        init_rot_(other.init_rot_),
        end_rot_(other.end_rot_),
91
        angular_vel_(other.angular_vel_),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
92
93
94
95
96
97
        T_min_(other.T_min_),
        T_max_(other.T_max_) {}

  quaternion_t computeAsQuaternion(const time_t t) const {
    if (Safe & !(T_min_ <= t && t <= T_max_)) {
      throw std::invalid_argument("can't evaluate bezier curve, time t is out of range");  // TODO
Pierre Fernbach's avatar
Pierre Fernbach committed
98
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
99
100
101
102
103
104
105
106
107
108
109
    if (t > T_max_) return end_rot_;
    if (t < T_min_) return init_rot_;
    Scalar u = (t - T_min_) / (T_max_ - T_min_);
    return init_rot_.slerp(u, end_rot_);
  }

  ///  \brief Evaluation of the SO3Linear at time t using Eigen slerp.
  ///  \param t : time when to evaluate the spline.
  ///  \return \f$x(t)\f$ point corresponding on spline at time t.
  virtual matrix3_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }

Pierre Fernbach's avatar
Pierre Fernbach committed
110
  /**
111
112
   * @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
113
114
115
116
   * @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
   */
117
  bool isApprox(const SO3Linear_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
118
119
    return  curves::isApprox<Numeric> (T_min_, other.min())
        && curves::isApprox<Numeric> (T_max_, other.max())
Pierre Fernbach's avatar
Pierre Fernbach committed
120
        && dim_ == other.dim()
121
122
        && init_rot_.toRotationMatrix().isApprox(other.init_rot_.toRotationMatrix(),prec)
        && end_rot_.toRotationMatrix().isApprox(other.end_rot_.toRotationMatrix(),prec);
Pierre Fernbach's avatar
Pierre Fernbach committed
123
124
  }

125
126
127
128
129
130
131
132
133
  virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
    const SO3Linear_t* other_cast = dynamic_cast<const SO3Linear_t*>(other);
    if(other_cast)
      return isApprox(*other_cast,prec);
    else
      return false;
  }


Pierre Fernbach's avatar
Pierre Fernbach committed
134
135
136
137
138
139
140
141
142
  virtual bool operator==(const SO3Linear_t& other) const {
    return isApprox(other);
  }

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


Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
143
144
145
146
147
  ///  \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 point3_t derivate(const time_t t, const std::size_t order) const {
148
149
150
151
    if ((t < T_min_ || t > T_max_) && Safe) {
      throw std::invalid_argument(
          "error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
152
153
154
155
156
157
    if (order > 1 || t > T_max_ || t < T_min_) {
      return point3_t::Zero(3);
    } else if (order == 1) {
      return angular_vel_;
    } else {
      throw std::invalid_argument("Order must be > 0 ");
Pierre Fernbach's avatar
Pierre Fernbach committed
158
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
159
160
  }

161
  SO3Linear_t compute_derivate(const std::size_t /*order*/) const {
162
163
164
    throw std::logic_error("Compute derivate for SO3Linear is not implemented yet.");
  }

165
166
167
168
169
170
171
172
  ///  \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.
  SO3Linear_t* compute_derivate_ptr(const std::size_t order) const {
    return new SO3Linear_t(compute_derivate(order));
  }


Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
173
174
175
176
177
178
179
180
181
182
  /*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_; }
183
184
185
  /// \brief Get the degree of the curve.
  /// \return \f$degree\f$, the degree of the curve.
  virtual std::size_t  degree() const {return 1;}
186
187
188
189
190
  matrix3_t getInitRotation()const {return init_rot_.toRotationMatrix();}
  matrix3_t getEndRotation()const {return end_rot_.toRotationMatrix();}
  matrix3_t getInitRotation() {return init_rot_.toRotationMatrix();}
  matrix3_t getEndRotation() {return end_rot_.toRotationMatrix();}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
191
192
193
194
195
  /*Helpers*/

  /*Attributes*/
  std::size_t dim_;  // const
  quaternion_t init_rot_, end_rot_;
196
  point3_t angular_vel_;  // const
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
197
198
199
200
201
202
203
  time_t T_min_, T_max_;  // const
  /*Attributes*/

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

  template <class Archive>
204
  void load(Archive& ar, const unsigned int version) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
205
206
    if (version) {
      // Do something depending on version ?
Pierre Fernbach's avatar
Pierre Fernbach committed
207
    }
208
    ar>> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
209
210
    ar>> boost::serialization::make_nvp("dim", dim_);
    matrix3_t init,end;
211
212
    ar >> boost::serialization::make_nvp("init_rotation", init);
    ar >> boost::serialization::make_nvp("end_rotation", end);
213
214
215
216
217
218
    init_rot_ = quaternion_t(init);
    end_rot_ = quaternion_t(end);
    ar >> boost::serialization::make_nvp("angular_vel", angular_vel_);
    ar >> boost::serialization::make_nvp("T_min", T_min_);
    ar >> boost::serialization::make_nvp("T_max", T_max_);

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
219
220
  }

221
222
223
224
225
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    if (version) {
      // Do something depending on version ?
    }
226
    ar<< BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
227
228
229
230
231
232
233
234
235
236
237
238
    ar<< boost::serialization::make_nvp("dim", dim_);
    matrix3_t init_matrix(getInitRotation());
    matrix3_t end_matrix(getEndRotation());
    ar<< boost::serialization::make_nvp("init_rotation", init_matrix);
    ar<< boost::serialization::make_nvp("end_rotation", end_matrix);
    ar<< boost::serialization::make_nvp("angular_vel", angular_vel_);
    ar<< boost::serialization::make_nvp("T_min", T_min_);
    ar<< boost::serialization::make_nvp("T_max", T_max_);
  }

  BOOST_SERIALIZATION_SPLIT_MEMBER()

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
  /// \brief Log: SO3 -> so3.
  ///
  /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
  ///
  /// Code from pinocchio.
  ///
  /// \param[in] R the rotation matrix.    ///
  /// \return The angular velocity vector associated to the rotation matrix.
  ///
  point3_t log3(const matrix3_t& R) {
    Scalar theta;
    static const Scalar PI_value = boost::math::constants::pi<Scalar>();

    point3_t res;
    const Scalar tr = R.trace();
    if (tr > Scalar(3))
      theta = 0;  // acos((3-1)/2)
    else if (tr < Scalar(-1))
      theta = PI_value;  // acos((-1-1)/2)
    else
      theta = acos((tr - Scalar(1)) / Scalar(2));
    assert(theta == theta && "theta contains some NaN");  // theta != NaN

    // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
    if (theta < PI_value - 1e-2) {
      const Scalar t = ((theta > std::pow(std::numeric_limits<Scalar>::epsilon(),
                                          Scalar(1) / Scalar(4)))  // precision of taylor serie of degree 3
                            ? theta / sin(theta)
                            : Scalar(1)) /
                       Scalar(2);
      res(0) = t * (R(2, 1) - R(1, 2));
      res(1) = t * (R(0, 2) - R(2, 0));
      res(2) = t * (R(1, 0) - R(0, 1));
    } else {
      // 1e-2: A low value is not required since the computation is
      // using explicit formula. However, the precision of this method
      // is the square root of the precision with the antisymmetric
      // method (Nominal case).
      const Scalar cphi = cos(theta - PI_value);
      const Scalar beta = theta * theta / (Scalar(1) + cphi);
      point3_t tmp((R.diagonal().array() + cphi) * beta);
      res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
      res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
      res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
Pierre Fernbach's avatar
Pierre Fernbach committed
283
284
    }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
285
286
    return res;
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
287

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
288
289
290
291
292
 private:
  void safe_check() {
    if (Safe) {
      if (T_min_ > T_max_) {
        throw std::invalid_argument("Tmin should be inferior to Tmax");
Pierre Fernbach's avatar
Pierre Fernbach committed
293
294
      }
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
295
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
296

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
297
};  // struct SO3Linear
Pierre Fernbach's avatar
Pierre Fernbach committed
298

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
299
}  // namespace curves
Pierre Fernbach's avatar
Pierre Fernbach committed
300

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
301
#endif  // _STRUCT_SO3_LINEAR_H