so3_smooth.h
Go to the documentation of this file.
1 #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
2 
3 #ifndef NDCURVES_SO3_SMOOTH_H
4 #define NDCURVES_SO3_SMOOTH_H
5 
6 #include <Eigen/Geometry>
7 #include <boost/math/constants/constants.hpp>
8 #include <boost/serialization/split_free.hpp>
9 #include <boost/serialization/vector.hpp>
10 #include <pinocchio/fwd.hpp>
11 #include <pinocchio/spatial/explog.hpp>
12 
13 #include "ndcurves/MathDefs.h"
15 #include "ndcurves/curve_abc.h"
16 #include "ndcurves/polynomial.h"
17 
18 namespace ndcurves {
19 
24 template <typename Time = double, typename Numeric = Time, bool Safe = false>
25 struct SO3Smooth : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> {
26  typedef Numeric Scalar;
27  typedef matrix3_t point_t;
28  typedef matrix3_t_cst_ref point_t_ref;
29  typedef point3_t point_derivate_t;
30  typedef Time time_t;
31  typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t;
32  typedef constant_curve<Time, Numeric, Safe, point_derivate_t>
33  curve_derivate_t;
34  typedef SO3Smooth<Time, Numeric, Safe> SO3Smooth_t;
35  typedef polynomial<Time, Numeric, Safe, point1_t> min_jerk_t;
36 
37  public:
38  /* Constructors - destructors */
42  SO3Smooth()
43  : curve_abc_t(),
44  init_rot_(point_t::Identity()),
45  end_rot_(point_t::Identity()),
46  t_min_(0.0),
47  t_max_(1.0),
48  min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
49  t_max_)),
50  rot_diff_(pinocchio::log3(init_rot_.transpose() * end_rot_)),
51  dt_(1e-3) {}
52 
54  SO3Smooth(const quaternion_t& init_quat, const quaternion_t& end_quat,
55  const time_t t_min, const time_t t_max)
56  : curve_abc_t(),
57  t_min_(t_min),
58  t_max_(t_max),
59  min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
60  t_max_)),
61  dt_(1e-3) {
62  quaternion_t tmp_init_quat = init_quat;
63  quaternion_t tmp_end_quat = end_quat;
64  tmp_init_quat.normalized();
65  tmp_end_quat.normalized();
66  init_rot_ = tmp_init_quat.toRotationMatrix();
67  end_rot_ = tmp_end_quat.toRotationMatrix();
68  rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
69  safe_check();
70  }
71 
74  SO3Smooth(const matrix3_t& init_rot, const matrix3_t& end_rot,
75  const time_t t_min, const time_t t_max)
76  : curve_abc_t(),
77  init_rot_(init_rot),
78  end_rot_(end_rot),
79  t_min_(t_min),
80  t_max_(t_max),
81  min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
82  t_max_)),
83  rot_diff_(pinocchio::log3(init_rot_.transpose() * end_rot_)),
84  dt_(1e-3) {
85  safe_check();
86  }
87 
90  SO3Smooth(const quaternion_t& init_rot, const quaternion_t& end_rot)
91  : curve_abc_t(),
92  t_min_(0.),
93  t_max_(1.),
94  min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
95  t_max_)),
96  dt_(1e-3) {
97  quaternion_t init_rot_normalized = init_rot.normalized();
98  quaternion_t end_rot_normalized = end_rot.normalized();
99  init_rot_ = init_rot_normalized.toRotationMatrix();
100  end_rot_ = end_rot_normalized.toRotationMatrix();
101  rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
102  safe_check();
103  }
104 
107  SO3Smooth(const matrix3_t& init_rot, const matrix3_t& end_rot)
108  : curve_abc_t(),
109  init_rot_(init_rot),
110  end_rot_(end_rot),
111  t_min_(0.),
112  t_max_(1.),
113  min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
114  t_max_)),
115  dt_(1e-3) {
116  rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
117  safe_check();
118  }
119 
120  // Generate functions:
121 
122  /* Generators. */
126  void generate() {
127  init_rot_.setIdentity();
128  end_rot_.setIdentity();
129  t_min_ = 0.0;
130  t_max_ = 1.0;
131  min_jerk_t::MinimumJerk(min_jerk_, point1_t(0.0), point1_t(1.0), t_min_,
132  t_max_);
133  rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
134  safe_check();
135  }
136 
138  void generate(const matrix3_t& init_rot, const matrix3_t& end_rot,
139  const time_t t_min, const time_t t_max) {
140  init_rot_ = init_rot;
141  end_rot_ = end_rot;
142  t_min_ = t_min;
143  t_max_ = t_max;
144  min_jerk_t::MinimumJerk(min_jerk_, point1_t(0.0), point1_t(1.0), t_min_,
145  t_max_);
146  rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
147  safe_check();
148  }
149 
152  void generate(const quaternion_t& init_rot, const quaternion_t& end_rot,
153  const time_t t_min, const time_t t_max) {
154  quaternion_t init_rot_normalized = init_rot.normalized();
155  quaternion_t end_rot_normalized = end_rot.normalized();
156  generate(init_rot_normalized.toRotationMatrix(),
157  end_rot_normalized.toRotationMatrix(), t_min, t_max);
158  }
159 
160  void generate(const quaternion_t& init_rot, const quaternion_t& end_rot) {
161  quaternion_t init_rot_normalized = init_rot.normalized();
162  quaternion_t end_rot_normalized = end_rot.normalized();
163  generate(init_rot_normalized.toRotationMatrix(),
164  end_rot_normalized.toRotationMatrix(), 0.0, 1.0);
165  }
166 
169  void generate(const matrix3_t& init_rot, const matrix3_t& end_rot) {
170  generate(init_rot, end_rot, 0.0, 1.0);
171  }
172 
174  virtual ~SO3Smooth() {}
175 
177  SO3Smooth(const SO3Smooth& other)
178  : curve_abc_t(),
179  init_rot_(other.init_rot_),
180  end_rot_(other.end_rot_),
181  t_min_(other.t_min_),
182  t_max_(other.t_max_),
183  min_jerk_(other.min_jerk_),
184  rot_diff_(other.rot_diff_),
185  dt_(1e-3) {}
186 
190  virtual point_t operator()(const time_t t) const {
191  if ((t < t_min_ || t > t_max_) && Safe) {
192  throw std::invalid_argument(
193  "error in SO3Smooth : time t to evaluate derivative should be in "
194  "range [Tmin, Tmax] of the curve");
195  }
196  return init_rot_ * pinocchio::exp3(min_jerk_(t)[0] * rot_diff_);
197  }
198 
208  bool isApprox(
209  const SO3Smooth_t& other,
210  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
211  return ndcurves::isApprox<Numeric>(t_min_, other.min()) &&
212  ndcurves::isApprox<Numeric>(t_max_, other.max()) &&
213  init_rot_.isApprox(other.init_rot_, prec) &&
214  end_rot_.isApprox(other.end_rot_, prec);
215  }
216 
217  virtual bool isApprox(
218  const curve_abc_t* other,
219  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
220  const SO3Smooth_t* other_cast = dynamic_cast<const SO3Smooth_t*>(other);
221  if (other_cast)
222  return isApprox(*other_cast, prec);
223  else
224  return false;
225  }
226 
227  virtual bool operator==(const SO3Smooth_t& other) const {
228  return isApprox(other);
229  }
230 
231  virtual bool operator!=(const SO3Smooth_t& other) const {
232  return !(*this == other);
233  }
234 
246  virtual point_derivate_t derivate(const time_t t,
247  const std::size_t order) const {
248  point_derivate_t out = point_derivate_t::Zero();
249  if ((t < t_min_ || t > t_max_) && Safe) {
250  throw std::invalid_argument(
251  "error in SO3Smooth : time t to evaluate derivative should be in "
252  "range [Tmin, Tmax] of the curve");
253  }
254  if (order > 2) {
255  throw std::invalid_argument("Order must be in {1, 2}.");
256  } else if (order == 2) {
257  double t1 = t;
258  double t2 = t + dt_;
259  if (t2 > t_max_) {
260  t1 = t - dt_;
261  t2 = t;
262  }
263  if (t1 < t_min_) {
264  t1 = t_min_;
265  t2 = t_max_;
266  }
267  if (t1 == t2) {
268  out.setZero();
269  } else {
270  out = (derivate(t1, 1) - derivate(t2, 1)) / (t2 - t1);
271  }
272  } else if (order == 1) {
273  matrix3_t jexp;
274  pinocchio::Jexp3(min_jerk_(t)[0] * rot_diff_, jexp);
275  out.noalias() =
276  init_rot_ * jexp * min_jerk_.derivate(t, 1)[0] * rot_diff_;
277  } else {
278  throw std::invalid_argument("Order must be > 0 ");
279  }
280  return out;
281  }
282 
283  curve_derivate_t compute_derivate(const std::size_t /*order*/) const {
284  throw std::runtime_error("This function has not been implemented yet.");
285  }
286 
291  curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
292  return new curve_derivate_t(compute_derivate(order));
293  }
294 
295  /*Helpers*/
298  std::size_t virtual dim() const { return 3; };
301  time_t min() const { return t_min_; }
304  time_t max() const { return t_max_; }
307  virtual std::size_t degree() const { return 5; }
308  matrix3_t_cst_ref get_init_rotation() const { return init_rot_; }
309  matrix3_t_cst_ref get_end_rotation() const { return end_rot_; }
310 
311  /*Attributes*/
312  point_t init_rot_;
313  point_t end_rot_;
314  time_t t_min_;
315  time_t t_max_;
316  min_jerk_t
317  min_jerk_;
318  point3_t
319  rot_diff_;
320  const double dt_;
323  // Serialization of the class
324  friend class boost::serialization::access;
325 
326  template <class Archive>
327  void load(Archive& ar, const unsigned int version) {
328  if (version) {
329  // Do something depending on version ?
330  }
331  ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
332  ar >> boost::serialization::make_nvp("init_rotation", init_rot_);
333  ar >> boost::serialization::make_nvp("end_rotation", end_rot_);
334  ar >> boost::serialization::make_nvp("t_min", t_min_);
335  ar >> boost::serialization::make_nvp("t_max", t_max_);
336  ar >> boost::serialization::make_nvp("min_jerk", min_jerk_);
337  ar >> boost::serialization::make_nvp("rot_diff", rot_diff_);
338  }
339 
340  template <class Archive>
341  void save(Archive& ar, const unsigned int version) const {
342  if (version) {
343  // Do something depending on version ?
344  }
345  ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
346  ar << boost::serialization::make_nvp("init_rotation", init_rot_);
347  ar << boost::serialization::make_nvp("end_rotation", end_rot_);
348  ar << boost::serialization::make_nvp("t_min", t_min_);
349  ar << boost::serialization::make_nvp("t_max", t_max_);
350  ar << boost::serialization::make_nvp("min_jerk", min_jerk_);
351  ar << boost::serialization::make_nvp("rot_diff", rot_diff_);
352  }
353 
354  BOOST_SERIALIZATION_SPLIT_MEMBER()
355 
356 
357  point3_t log3(const matrix3_t& R) {
366  Scalar theta;
367  static const Scalar PI_value = boost::math::constants::pi<Scalar>();
368 
369  point3_t res;
370  const Scalar tr = R.trace();
371  if (tr > Scalar(3))
372  theta = 0; // acos((3-1)/2)
373  else if (tr < Scalar(-1))
374  theta = PI_value; // acos((-1-1)/2)
375  else
376  theta = acos((tr - Scalar(1)) / Scalar(2));
377  if (!std::isfinite(theta)) {
378  throw std::runtime_error("theta contains some NaN");
379  }
380 
381  // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
382  if (theta < PI_value - 1e-2) {
383  const Scalar t =
384  ((theta >
385  std::pow(std::numeric_limits<Scalar>::epsilon(),
386  Scalar(1) /
387  Scalar(4))) // precision of taylor serie of degree 3
388  ? theta / sin(theta)
389  : Scalar(1)) /
390  Scalar(2);
391  res(0) = t * (R(2, 1) - R(1, 2));
392  res(1) = t * (R(0, 2) - R(2, 0));
393  res(2) = t * (R(1, 0) - R(0, 1));
394  } else {
395  // 1e-2: A low value is not required since the computation is
396  // using explicit formula. However, the precision of this method
397  // is the square root of the precision with the antisymmetric
398  // method (Nominal case).
399  const Scalar cphi = cos(theta - PI_value);
400  const Scalar beta = theta * theta / (Scalar(1) + cphi);
401  point3_t tmp((R.diagonal().array() + cphi) * beta);
402  res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) *
403  (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
404  res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) *
405  (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
406  res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) *
407  (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
408  }
409 
410  return res;
411  }
412 
413  private:
414  void safe_check() {
415  if (Safe) {
416  if (t_min_ > t_max_) {
417  throw std::invalid_argument("Tmin should be inferior to Tmax");
418  }
419  }
420  }
421 
422 }; // struct SO3Smooth
423 
424 } // namespace ndcurves
425 
427  SINGLE_ARG(typename Time, typename Numeric, bool Safe),
429 
430 #endif // NDCURVES_SO3_SMOOTH_H
431 #endif // CURVES_WITH_PINOCCHIO_SUPPORT
#define SINGLE_ARG(...)
Definition: archive.hpp:23
Definition: bernstein.h:20
Definition: fwd.h:54
Eigen::Quaternion< double > quaternion_t
Definition: fwd.h:76
Eigen::Matrix< double, 1, 1 > point1_t
Definition: fwd.h:66
Eigen::Ref< const matrix3_t > matrix3_t_cst_ref
Definition: fwd.h:81
Definition of a cubic spline.
interface for a Curve of arbitrary dimension.
Eigen::Vector3d point3_t
Definition: fwd.h:71
curve_abc< double, double, true, pointX_t, pointX_t > curve_abc_t
Definition: fwd.h:85
double Time
Definition: effector_spline.h:27
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:74
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
void load(Archive &ar, Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &m, const unsigned int)
Definition: eigen-matrix.hpp:63
bool isApprox(const T a, const T b, const T eps=1e-6)
Definition: curve_abc.h:26
double Numeric
Definition: effector_spline.h:26
SO3Smooth< double, double, true > SO3Smooth_t
Definition: fwd.h:131
void save(Archive &ar, const Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &m, const unsigned int)
Definition: eigen-matrix.hpp:50
class allowing to create a constant_curve curve.