so3_linear.h
Go to the documentation of this file.
1 #ifndef _STRUCT_SO3_LINEAR_H
2 #define _STRUCT_SO3_LINEAR_H
3 
4 #include <Eigen/Geometry>
5 #include <boost/math/constants/constants.hpp>
6 #include <boost/serialization/split_free.hpp>
7 #include <boost/serialization/vector.hpp>
8 
9 #include "MathDefs.h"
10 #include "constant_curve.h"
11 #include "curve_abc.h"
12 
13 namespace ndcurves {
14 
20 template <typename Time = double, typename Numeric = Time, bool Safe = false>
21 struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> {
22  typedef Numeric Scalar;
23  typedef matrix3_t point_t;
25  typedef Eigen::Quaternion<Scalar> quaternion_t;
26  typedef Time time_t;
31 
32  public:
33  /* Constructors - destructors */
38  : curve_abc_t(),
39  dim_(3),
40  init_rot_(),
41  end_rot_(),
42  angular_vel_(),
43  T_min_(0),
44  T_max_(0) {}
45 
47  SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot,
48  const time_t t_min, const time_t t_max)
49  : curve_abc_t(),
50  dim_(3),
51  init_rot_(init_rot),
52  end_rot_(end_rot),
53  angular_vel_(computeAngularVelocity(init_rot.toRotationMatrix(),
54  end_rot.toRotationMatrix(), t_min,
55  t_max)),
56  T_min_(t_min),
57  T_max_(t_max) {
58  safe_check();
59  }
60 
63  SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot,
64  const time_t t_min, const time_t t_max)
65  : curve_abc_t(),
66  dim_(3),
67  init_rot_(quaternion_t(init_rot)),
68  end_rot_(quaternion_t(end_rot)),
69  angular_vel_(computeAngularVelocity(init_rot, end_rot, t_min, t_max)),
70  T_min_(t_min),
71  T_max_(t_max) {
72  safe_check();
73  }
74 
77  SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot)
78  : curve_abc_t(),
79  dim_(3),
80  init_rot_(init_rot),
81  end_rot_(end_rot),
83  init_rot.toRotationMatrix(), end_rot.toRotationMatrix(), 0., 1.)),
84  T_min_(0.),
85  T_max_(1.) {
86  safe_check();
87  }
88 
91  SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot)
92  : curve_abc_t(),
93  dim_(3),
94  init_rot_(quaternion_t(init_rot)),
95  end_rot_(quaternion_t(end_rot)),
96  angular_vel_(computeAngularVelocity(init_rot, end_rot, 0., 1.)),
97  T_min_(0.),
98  T_max_(1.) {
99  safe_check();
100  }
101 
103  virtual ~SO3Linear() {}
104 
105  SO3Linear(const SO3Linear& other)
106  : dim_(other.dim_),
107  init_rot_(other.init_rot_),
108  end_rot_(other.end_rot_),
109  angular_vel_(other.angular_vel_),
110  T_min_(other.T_min_),
111  T_max_(other.T_max_) {}
112 
114  const matrix3_t& end_rot, const double t_min,
115  const double t_max) {
116  if (t_min == t_max) {
117  return point3_t::Zero();
118  } else {
119  return log3(init_rot.transpose() * end_rot) / (t_max - t_min);
120  }
121  }
122 
124  if (Safe & !(T_min_ <= t && t <= T_max_)) {
125  throw std::invalid_argument(
126  "can't evaluate bezier curve, time t is out of range"); // TODO
127  }
128  if (t >= T_max_) return end_rot_;
129  if (t <= T_min_) return init_rot_;
130  Scalar u = (t - T_min_) / (T_max_ - T_min_);
131  return init_rot_.slerp(u, end_rot_);
132  }
133 
137  virtual point_t operator()(const time_t t) const {
138  return computeAsQuaternion(t).toRotationMatrix();
139  }
140 
150  bool isApprox(
151  const SO3Linear_t& other,
152  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
153  return ndcurves::isApprox<Numeric>(T_min_, other.min()) &&
154  ndcurves::isApprox<Numeric>(T_max_, other.max()) &&
155  dim_ == other.dim() &&
156  init_rot_.toRotationMatrix().isApprox(
157  other.init_rot_.toRotationMatrix(), prec) &&
158  end_rot_.toRotationMatrix().isApprox(
159  other.end_rot_.toRotationMatrix(), prec);
160  }
161 
162  virtual bool isApprox(
163  const curve_abc_t* other,
164  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
165  const SO3Linear_t* other_cast = dynamic_cast<const SO3Linear_t*>(other);
166  if (other_cast)
167  return isApprox(*other_cast, prec);
168  else
169  return false;
170  }
171 
172  virtual bool operator==(const SO3Linear_t& other) const {
173  return isApprox(other);
174  }
175 
176  virtual bool operator!=(const SO3Linear_t& other) const {
177  return !(*this == other);
178  }
179 
186  const std::size_t order) const {
187  if ((t < T_min_ || t > T_max_) && Safe) {
188  throw std::invalid_argument(
189  "error in SO3_linear : time t to evaluate derivative should be in "
190  "range [Tmin, Tmax] of the curve");
191  }
192  if (order > 1 || t > T_max_ || t < T_min_) {
193  return point3_t::Zero(3);
194  } else if (order == 1) {
195  return angular_vel_;
196  } else {
197  throw std::invalid_argument("Order must be > 0 ");
198  }
199  }
200 
201  curve_derivate_t compute_derivate(const std::size_t order) const {
202  return curve_derivate_t(derivate(T_min_, order), T_min_, T_max_);
203  }
204 
209  curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
210  return new curve_derivate_t(compute_derivate(order));
211  }
212 
213  /*Helpers*/
216  std::size_t virtual dim() const { return dim_; };
219  time_t min() const { return T_min_; }
222  time_t max() const { return T_max_; }
225  virtual std::size_t degree() const { return 1; }
226  matrix3_t getInitRotation() const { return init_rot_.toRotationMatrix(); }
227  matrix3_t getEndRotation() const { return end_rot_.toRotationMatrix(); }
228  matrix3_t getInitRotation() { return init_rot_.toRotationMatrix(); }
229  matrix3_t getEndRotation() { return end_rot_.toRotationMatrix(); }
230 
231  /*Helpers*/
232 
233  /*Attributes*/
234  std::size_t dim_; // const
237  time_t T_min_, T_max_; // const
238  /*Attributes*/
239 
240  // Serialization of the class
242 
243  template <class Archive>
244  void load(Archive& ar, const unsigned int version) {
245  if (version) {
246  // Do something depending on version ?
247  }
248  ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
249  ar >> boost::serialization::make_nvp("dim", dim_);
250  matrix3_t init, end;
251  ar >> boost::serialization::make_nvp("init_rotation", init);
252  ar >> boost::serialization::make_nvp("end_rotation", end);
253  init_rot_ = quaternion_t(init);
254  end_rot_ = quaternion_t(end);
255  ar >> boost::serialization::make_nvp("angular_vel", angular_vel_);
256  ar >> boost::serialization::make_nvp("T_min", T_min_);
257  ar >> boost::serialization::make_nvp("T_max", T_max_);
258  }
259 
260  template <class Archive>
261  void save(Archive& ar, const unsigned int version) const {
262  if (version) {
263  // Do something depending on version ?
264  }
265  ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
266  ar << boost::serialization::make_nvp("dim", dim_);
267  matrix3_t init_matrix(getInitRotation());
268  matrix3_t end_matrix(getEndRotation());
269  ar << boost::serialization::make_nvp("init_rotation", init_matrix);
270  ar << boost::serialization::make_nvp("end_rotation", end_matrix);
271  ar << boost::serialization::make_nvp("angular_vel", angular_vel_);
272  ar << boost::serialization::make_nvp("T_min", T_min_);
273  ar << boost::serialization::make_nvp("T_max", T_max_);
274  }
275 
276  BOOST_SERIALIZATION_SPLIT_MEMBER()
277 
278 
287  point3_t log3(const matrix3_t& R) {
288  Scalar theta;
289  static const Scalar PI_value = boost::math::constants::pi<Scalar>();
290 
291  point3_t res;
292  const Scalar tr = R.trace();
293  if (tr > Scalar(3))
294  theta = 0; // acos((3-1)/2)
295  else if (tr < Scalar(-1))
296  theta = PI_value; // acos((-1-1)/2)
297  else
298  theta = acos((tr - Scalar(1)) / Scalar(2));
299  if (!std::isfinite(theta)) {
300  throw std::runtime_error("theta contains some NaN");
301  }
302 
303  // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
304  if (theta < PI_value - 1e-2) {
305  const Scalar t =
306  ((theta >
307  std::pow(std::numeric_limits<Scalar>::epsilon(),
308  Scalar(1) /
309  Scalar(4))) // precision of taylor serie of degree 3
310  ? theta / sin(theta)
311  : Scalar(1)) /
312  Scalar(2);
313  res(0) = t * (R(2, 1) - R(1, 2));
314  res(1) = t * (R(0, 2) - R(2, 0));
315  res(2) = t * (R(1, 0) - R(0, 1));
316  } else {
317  // 1e-2: A low value is not required since the computation is
318  // using explicit formula. However, the precision of this method
319  // is the square root of the precision with the antisymmetric
320  // method (Nominal case).
321  const Scalar cphi = cos(theta - PI_value);
322  const Scalar beta = theta * theta / (Scalar(1) + cphi);
323  point3_t tmp((R.diagonal().array() + cphi) * beta);
324  res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) *
325  (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
326  res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) *
327  (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
328  res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) *
329  (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
330  }
331 
332  return res;
333  }
334 
335  private:
336  void safe_check() {
337  if (Safe) {
338  if (T_min_ > T_max_) {
339  throw std::invalid_argument("Tmin should be inferior to Tmax");
340  }
341  }
342  }
343 
344 }; // struct SO3Linear
345 
346 } // namespace ndcurves
347 
349  SINGLE_ARG(typename Time, typename Numeric, bool Safe),
351 
352 #endif // _STRUCT_SO3_LINEAR_H
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
#define SINGLE_ARG(...)
Definition: archive.hpp:23
class allowing to create a constant_curve curve.
interface for a Curve of arbitrary dimension.
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
Definition: bernstein.h:20
Eigen::Vector3d point3_t
Definition: fwd.h:63
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:70
Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion.
Definition: so3_linear.h:21
virtual std::size_t degree() const
Get the degree of the curve.
Definition: so3_linear.h:225
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: so3_linear.h:27
void load(Archive &ar, const unsigned int version)
Definition: so3_linear.h:244
SO3Linear(const SO3Linear &other)
Definition: so3_linear.h:105
Eigen::Quaternion< Scalar > quaternion_t
Definition: so3_linear.h:25
point3_t computeAngularVelocity(const matrix3_t &init_rot, const matrix3_t &end_rot, const double t_min, const double t_max)
Definition: so3_linear.h:113
time_t T_min_
Definition: so3_linear.h:237
quaternion_t init_rot_
Definition: so3_linear.h:235
time_t max() const
Get the maximum time for which the curve is defined.
Definition: so3_linear.h:222
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: so3_linear.h:185
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: so3_linear.h:162
virtual ~SO3Linear()
Destructor.
Definition: so3_linear.h:103
matrix3_t point_t
Definition: so3_linear.h:23
SO3Linear(const matrix3_t &init_rot, const matrix3_t &end_rot, const time_t t_min, const time_t t_max)
constructor with initial and final rotation expressed as rotation matrix and time bounds
Definition: so3_linear.h:63
matrix3_t getEndRotation() const
Definition: so3_linear.h:227
SO3Linear()
Empty constructor. Curve obtained this way can not perform other class functions.
Definition: so3_linear.h:37
virtual bool operator!=(const SO3Linear_t &other) const
Definition: so3_linear.h:176
quaternion_t end_rot_
Definition: so3_linear.h:235
virtual point_t operator()(const time_t t) const
Evaluation of the SO3Linear at time t using Eigen slerp.
Definition: so3_linear.h:137
matrix3_t getEndRotation()
Definition: so3_linear.h:229
matrix3_t getInitRotation() const
Definition: so3_linear.h:226
Time time_t
Definition: so3_linear.h:26
curve_derivate_t compute_derivate(const std::size_t order) const
Definition: so3_linear.h:201
time_t T_max_
Definition: so3_linear.h:237
SO3Linear(const quaternion_t &init_rot, const quaternion_t &end_rot, const time_t t_min, const time_t t_max)
constructor with initial and final rotation and time bounds
Definition: so3_linear.h:47
void save(Archive &ar, const unsigned int version) const
Definition: so3_linear.h:261
Numeric Scalar
Definition: so3_linear.h:22
matrix3_t getInitRotation()
Definition: so3_linear.h:228
point3_t point_derivate_t
Definition: so3_linear.h:24
point3_t log3(const matrix3_t &R)
Log: SO3 -> so3.
Definition: so3_linear.h:287
quaternion_t computeAsQuaternion(const time_t t) const
Definition: so3_linear.h:123
friend class boost::serialization::access
Definition: so3_linear.h:241
bool isApprox(const SO3Linear_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: so3_linear.h:150
curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: so3_linear.h:209
virtual std::size_t dim() const
Get dimension of curve.
Definition: so3_linear.h:216
time_t min() const
Get the minimum time for which the curve is defined.
Definition: so3_linear.h:219
SO3Linear(const quaternion_t &init_rot, const quaternion_t &end_rot)
constructor with initial and final rotation, time bounds are set to [0;1]
Definition: so3_linear.h:77
SO3Linear(const matrix3_t &init_rot, const matrix3_t &end_rot)
constructor with initial and final rotation expressed as rotation matrix, time bounds are set to [0;1...
Definition: so3_linear.h:91
point3_t angular_vel_
Definition: so3_linear.h:236
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: so3_linear.h:30
std::size_t dim_
Definition: so3_linear.h:234
virtual bool operator==(const SO3Linear_t &other) const
Definition: so3_linear.h:172
constant_curve< Time, Numeric, Safe, point_derivate_t > curve_derivate_t
Definition: so3_linear.h:29
Represents a constant_curve curve, always returning the same value and a null derivative.
Definition: constant_curve.h:23
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37