so3_linear.h 9.06 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
25
26
27
28
29
30
31
32
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;
  typedef Eigen::Quaternion<Scalar> quaternion_t;
  typedef Time time_t;
  typedef curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> curve_abc_t;
  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
33
  ///
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
  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)
      : curve_abc_t(), dim_(3), init_rot_(init_rot), end_rot_(end_rot), angular_vel_(), T_min_(t_min), T_max_(t_max) {
    angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix()) / (T_max_ - T_min_);
    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)),
        angular_vel_(),
        T_min_(t_min),
        T_max_(t_max) {
    angular_vel_ = log3(init_rot.transpose() * end_rot) / (T_max_ - T_min_);
    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)
      : curve_abc_t(), dim_(3), init_rot_(init_rot), end_rot_(end_rot), angular_vel_(), T_min_(0.), T_max_(1.) {
    angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix());
  }

  /// \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)),
        angular_vel_(),
        T_min_(0.),
        T_max_(1.) {
    angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix());
  }

  /// \brief Destructor
  ~SO3Linear() {
Pierre Fernbach's avatar
Pierre Fernbach committed
76
    // NOTHING
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
77
78
79
80
81
82
83
84
85
86
87
88
  }

  SO3Linear(const SO3Linear& other)
      : dim_(other.dim_),
        init_rot_(other.init_rot_),
        end_rot_(other.end_rot_),
        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
89
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
    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(); }

  ///  \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 {
106
107
108
109
    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
110
111
112
113
114
115
    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
116
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
117
118
119
120
121
122
123
124
125
126
127
128
  }

  /*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_; }
129
130
131
132
133
  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
134
135
136
137
138
139
140
141
142
143
144
145
146
  /*Helpers*/

  /*Attributes*/
  std::size_t dim_;  // const
  quaternion_t init_rot_, end_rot_;
  point3_t angular_vel_;
  time_t T_min_, T_max_;  // const
  /*Attributes*/

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

  template <class Archive>
147
  void load(Archive& ar, const unsigned int version) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
148
149
    if (version) {
      // Do something depending on version ?
Pierre Fernbach's avatar
Pierre Fernbach committed
150
    }
151
152
    ar>> boost::serialization::make_nvp("dim", dim_);
    matrix3_t init,end;
153
154
    ar >> boost::serialization::make_nvp("init_rotation", init);
    ar >> boost::serialization::make_nvp("end_rotation", end);
155
156
157
158
159
160
    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
161
162
  }

163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    if (version) {
      // Do something depending on version ?
    }
    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
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
  /// \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
224
225
    }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
226
227
    return res;
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
228

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
229
230
231
232
233
 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
234
235
      }
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
236
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
237

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
238
};  // struct SO3Linear
Pierre Fernbach's avatar
Pierre Fernbach committed
239

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
240
}  // namespace curves
Pierre Fernbach's avatar
Pierre Fernbach committed
241

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