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 "MathDefs.h"
5 
6 #include "curve_abc.h"
7 #include "constant_curve.h"
8 #include <Eigen/Geometry>
9 #include <boost/math/constants/constants.hpp>
10 
11 #include <boost/serialization/split_free.hpp>
12 #include <boost/serialization/vector.hpp>
13 
14 namespace ndcurves {
15 
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;
30 
31 
32  public:
33  /* Constructors - destructors */
36  SO3Linear() : curve_abc_t(), dim_(3), init_rot_(), end_rot_(), angular_vel_(), T_min_(0), T_max_(0) {}
37 
39  SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot, const time_t t_min, const time_t t_max)
40  : curve_abc_t(),
41  dim_(3),
42  init_rot_(init_rot),
43  end_rot_(end_rot),
44  angular_vel_(computeAngularVelocity(init_rot.toRotationMatrix(), end_rot.toRotationMatrix(), t_min, t_max)),
45  T_min_(t_min),
46  T_max_(t_max) {
47  safe_check();
48  }
49 
51  SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot, const time_t t_min, const time_t t_max)
52  : curve_abc_t(),
53  dim_(3),
54  init_rot_(quaternion_t(init_rot)),
55  end_rot_(quaternion_t(end_rot)),
56  angular_vel_(computeAngularVelocity(init_rot, end_rot, t_min, t_max)),
57  T_min_(t_min),
58  T_max_(t_max) {
59  safe_check();
60  }
61 
63  SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot)
64  : curve_abc_t(),
65  dim_(3),
66  init_rot_(init_rot),
67  end_rot_(end_rot),
68  angular_vel_(computeAngularVelocity(init_rot.toRotationMatrix(), end_rot.toRotationMatrix(), 0., 1.)),
69  T_min_(0.),
70  T_max_(1.) {
71  safe_check();
72  }
73 
75  SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot)
76  : curve_abc_t(),
77  dim_(3),
78  init_rot_(quaternion_t(init_rot)),
79  end_rot_(quaternion_t(end_rot)),
80  angular_vel_(computeAngularVelocity(init_rot, end_rot, 0., 1.)),
81  T_min_(0.),
82  T_max_(1.) {
83  safe_check();
84  }
85 
88  // NOTHING
89  }
90 
91  SO3Linear(const SO3Linear& other)
92  : dim_(other.dim_),
93  init_rot_(other.init_rot_),
94  end_rot_(other.end_rot_),
96  T_min_(other.T_min_),
97  T_max_(other.T_max_) {}
98 
99  point3_t computeAngularVelocity(const matrix3_t& init_rot, const matrix3_t& end_rot, const double t_min, const double t_max){
100  if(t_min == t_max){
101  return point3_t::Zero();
102  }else{
103  return log3(init_rot.transpose() * end_rot) / (t_max - t_min);
104  }
105  }
106 
107  quaternion_t computeAsQuaternion(const time_t t) const {
108  if (Safe & !(T_min_ <= t && t <= T_max_)) {
109  throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
110  }
111  if (t >= T_max_) return end_rot_;
112  if (t <= T_min_) return init_rot_;
113  Scalar u = (t - T_min_) / (T_max_ - T_min_);
114  return init_rot_.slerp(u, end_rot_);
115  }
116 
120  virtual point_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
121 
130  bool isApprox(const SO3Linear_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
131  return ndcurves::isApprox<Numeric>(T_min_, other.min()) && ndcurves::isApprox<Numeric>(T_max_, other.max()) &&
132  dim_ == other.dim() && init_rot_.toRotationMatrix().isApprox(other.init_rot_.toRotationMatrix(), prec) &&
133  end_rot_.toRotationMatrix().isApprox(other.end_rot_.toRotationMatrix(), prec);
134  }
135 
136  virtual bool isApprox(const curve_abc_t* other,
137  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
138  const SO3Linear_t* other_cast = dynamic_cast<const SO3Linear_t*>(other);
139  if (other_cast)
140  return isApprox(*other_cast, prec);
141  else
142  return false;
143  }
144 
145  virtual bool operator==(const SO3Linear_t& other) const { return isApprox(other); }
146 
147  virtual bool operator!=(const SO3Linear_t& other) const { return !(*this == other); }
148 
153  virtual point_derivate_t derivate(const time_t t, const std::size_t order) const {
154  if ((t < T_min_ || t > T_max_) && Safe) {
155  throw std::invalid_argument(
156  "error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
157  }
158  if (order > 1 || t > T_max_ || t < T_min_) {
159  return point3_t::Zero(3);
160  } else if (order == 1) {
161  return angular_vel_;
162  } else {
163  throw std::invalid_argument("Order must be > 0 ");
164  }
165  }
166 
167  curve_derivate_t compute_derivate(const std::size_t order) const {
168  return curve_derivate_t(derivate(T_min_, order), T_min_, T_max_);
169  }
170 
174  curve_derivate_t* compute_derivate_ptr(const std::size_t order) const { return new curve_derivate_t(compute_derivate(order)); }
175 
176  /*Helpers*/
179  std::size_t virtual dim() const { return dim_; };
182  time_t min() const { return T_min_; }
185  time_t max() const { return T_max_; }
188  virtual std::size_t degree() const { return 1; }
189  matrix3_t getInitRotation() const { return init_rot_.toRotationMatrix(); }
190  matrix3_t getEndRotation() const { return end_rot_.toRotationMatrix(); }
191  matrix3_t getInitRotation() { return init_rot_.toRotationMatrix(); }
192  matrix3_t getEndRotation() { return end_rot_.toRotationMatrix(); }
193 
194  /*Helpers*/
195 
196  /*Attributes*/
197  std::size_t dim_; // const
198  quaternion_t init_rot_, end_rot_;
200  time_t T_min_, T_max_; // const
201  /*Attributes*/
202 
203  // Serialization of the class
205 
206  template <class Archive>
207  void load(Archive& ar, const unsigned int version) {
208  if (version) {
209  // Do something depending on version ?
210  }
211  ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
212  ar >> boost::serialization::make_nvp("dim", dim_);
213  matrix3_t init, end;
214  ar >> boost::serialization::make_nvp("init_rotation", init);
215  ar >> boost::serialization::make_nvp("end_rotation", end);
216  init_rot_ = quaternion_t(init);
217  end_rot_ = quaternion_t(end);
218  ar >> boost::serialization::make_nvp("angular_vel", angular_vel_);
219  ar >> boost::serialization::make_nvp("T_min", T_min_);
220  ar >> boost::serialization::make_nvp("T_max", T_max_);
221  }
222 
223  template <class Archive>
224  void save(Archive& ar, const unsigned int version) const {
225  if (version) {
226  // Do something depending on version ?
227  }
228  ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
229  ar << boost::serialization::make_nvp("dim", dim_);
230  matrix3_t init_matrix(getInitRotation());
231  matrix3_t end_matrix(getEndRotation());
232  ar << boost::serialization::make_nvp("init_rotation", init_matrix);
233  ar << boost::serialization::make_nvp("end_rotation", end_matrix);
234  ar << boost::serialization::make_nvp("angular_vel", angular_vel_);
235  ar << boost::serialization::make_nvp("T_min", T_min_);
236  ar << boost::serialization::make_nvp("T_max", T_max_);
237  }
238 
239  BOOST_SERIALIZATION_SPLIT_MEMBER()
240 
241 
242  point3_t log3(const matrix3_t& R) {
251  Scalar theta;
252  static const Scalar PI_value = boost::math::constants::pi<Scalar>();
253 
254  point3_t res;
255  const Scalar tr = R.trace();
256  if (tr > Scalar(3))
257  theta = 0; // acos((3-1)/2)
258  else if (tr < Scalar(-1))
259  theta = PI_value; // acos((-1-1)/2)
260  else
261  theta = acos((tr - Scalar(1)) / Scalar(2));
262  if (!std::isfinite(theta)) {
263  throw std::runtime_error("theta contains some NaN");
264  }
265 
266  // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
267  if (theta < PI_value - 1e-2) {
268  const Scalar t = ((theta > std::pow(std::numeric_limits<Scalar>::epsilon(),
269  Scalar(1) / Scalar(4))) // precision of taylor serie of degree 3
270  ? theta / sin(theta)
271  : Scalar(1)) /
272  Scalar(2);
273  res(0) = t * (R(2, 1) - R(1, 2));
274  res(1) = t * (R(0, 2) - R(2, 0));
275  res(2) = t * (R(1, 0) - R(0, 1));
276  } else {
277  // 1e-2: A low value is not required since the computation is
278  // using explicit formula. However, the precision of this method
279  // is the square root of the precision with the antisymmetric
280  // method (Nominal case).
281  const Scalar cphi = cos(theta - PI_value);
282  const Scalar beta = theta * theta / (Scalar(1) + cphi);
283  point3_t tmp((R.diagonal().array() + cphi) * beta);
284  res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
285  res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
286  res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
287  }
288 
289  return res;
290  }
291 
292  private:
293  void safe_check() {
294  if (Safe) {
295  if (T_min_ > T_max_) {
296  throw std::invalid_argument("Tmin should be inferior to Tmax");
297  }
298  }
299  }
300 
301 }; // struct SO3Linear
302 
303 } // namespace ndcurves
304 
305 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe),
307 
308 #endif // _STRUCT_SO3_LINEAR_H
Definition: bernstein.h:20
time_t T_min_
Definition: so3_linear.h:200
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: so3_linear.h:29
virtual bool operator==(const SO3Linear_t &other) const
Definition: so3_linear.h:145
quaternion_t init_rot_
Definition: so3_linear.h:198
virtual bool operator!=(const SO3Linear_t &other) const
Definition: so3_linear.h:147
matrix3_t point_t
Definition: so3_linear.h:23
virtual point_t operator()(const time_t t) const
Evaluation of the SO3Linear at time t using Eigen slerp.
Definition: so3_linear.h:120
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:153
~SO3Linear()
Destructor.
Definition: so3_linear.h:87
interface for a Curve of arbitrary dimension.
Eigen::Vector3d point3_t
Definition: fwd.h:58
virtual std::size_t degree() const
Get the degree of the curve.
Definition: so3_linear.h:188
matrix3_t getInitRotation() const
Definition: so3_linear.h:189
matrix3_t getInitRotation()
Definition: so3_linear.h:191
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:63
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equal given a precision treshold Only two curves ...
Definition: so3_linear.h:136
matrix3_t getEndRotation()
Definition: so3_linear.h:192
time_t T_max_
Definition: so3_linear.h:200
SO3Linear()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: so3_linear.h:36
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:99
double Time
Definition: effector_spline.h:27
constant_curve< Time, Numeric, Safe, point_derivate_t > curve_derivate_t
Definition: so3_linear.h:28
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: so3_linear.h:27
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:65
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:75
Numeric Scalar
Definition: so3_linear.h:22
curve_derivate_t compute_derivate(const std::size_t order) const
Definition: so3_linear.h:167
std::size_t dim_
Definition: so3_linear.h:197
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:51
time_t min() const
Get the minimum time for which the curve is defined.
Definition: so3_linear.h:182
friend class boost::serialization::access
Definition: so3_linear.h:204
quaternion_t computeAsQuaternion(const time_t t) const
Definition: so3_linear.h:107
Time time_t
Definition: so3_linear.h:26
Eigen::Quaternion< Scalar > quaternion_t
Definition: so3_linear.h:25
matrix3_t getEndRotation() const
Definition: so3_linear.h:190
double Numeric
Definition: effector_spline.h:26
void save(Archive &ar, const unsigned int version) const
Definition: so3_linear.h:224
Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion.
Definition: fwd.h:46
time_t max() const
Get the maximum time for which the curve is defined.
Definition: so3_linear.h:185
SO3Linear(const SO3Linear &other)
Definition: so3_linear.h:91
virtual std::size_t dim() const
Get dimension of curve.
Definition: so3_linear.h:179
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:130
point3_t point_derivate_t
Definition: so3_linear.h:24
point3_t log3(const matrix3_t &R)
Log: SO3 -> so3.
Definition: so3_linear.h:250
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:39
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
class allowing to create a constant_curve curve.
point3_t angular_vel_
Definition: so3_linear.h:199
Represents a constant_curve curve, always returning the same value and a null derivative.
Definition: constant_curve.h:20
curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: so3_linear.h:174
void load(Archive &ar, const unsigned int version)
Definition: so3_linear.h:207
quaternion_t end_rot_
Definition: so3_linear.h:198