effector_spline_rotation.h
Go to the documentation of this file.
1 
19 #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION
20 #define _CLASS_EFFECTOR_SPLINE_ROTATION
21 
22 #include <Eigen/Geometry>
23 
24 #include "ndcurves/curve_abc.h"
26 
27 namespace ndcurves {
28 namespace helpers {
29 typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
30 typedef Eigen::Ref<quat_t> quat_ref_t;
31 typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
32 typedef Eigen::Matrix<Numeric, 7, 1> config_t;
34 typedef std::pair<Numeric, quat_t> waypoint_quat_t;
35 typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
36 typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
39 typedef std::pair<Numeric, point_one_dim_t> waypoint_one_dim_t;
40 typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
41 
43  public:
44  rotation_spline(quat_ref_const_t quat_from = quat_t(0, 0, 0, 1),
45  quat_ref_const_t quat_to = quat_t(0, 0, 0, 1),
46  const double min = 0., const double max = 1.)
47  : curve_abc_quat_t(),
48  quat_from_(quat_from.data()),
49  quat_to_(quat_to.data()),
50  dim_(4),
51  min_(min),
52  max_(max),
54 
56 
57  /* Copy Constructors / operator=*/
59  quat_from_ = from.quat_from_;
60  quat_to_ = from.quat_to_;
61  dim_ = from.dim_;
62  min_ = from.min_;
63  max_ = from.max_;
65  return *this;
66  }
67  /* Copy Constructors / operator=*/
68 
69  quat_t operator()(const Numeric t) const {
70  if (t <= min()) return quat_from_.coeffs();
71  if (t >= max()) return quat_to_.coeffs();
72  // normalize u
73  Numeric u = (t - min()) / (max() - min());
74  // reparametrize u
75  return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
76  }
77 
87  bool isApprox(
88  const rotation_spline& other,
89  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
90  return ndcurves::isApprox<Numeric>(min_, other.min_) &&
91  ndcurves::isApprox<Numeric>(max_, other.max_) &&
92  dim_ == other.dim_ && quat_from_.isApprox(other.quat_from_, prec) &&
93  quat_to_.isApprox(other.quat_to_, prec) &&
95  }
96 
97  virtual bool isApprox(
98  const curve_abc_quat_t* other,
99  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
100  const rotation_spline* other_cast =
101  dynamic_cast<const rotation_spline*>(other);
102  if (other_cast)
103  return isApprox(*other_cast, prec);
104  else
105  return false;
106  }
107 
108  virtual bool operator==(const rotation_spline& other) const {
109  return isApprox(other);
110  }
111 
112  virtual bool operator!=(const rotation_spline& other) const {
113  return !(*this == other);
114  }
115 
116  virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const {
117  throw std::runtime_error(
118  "TODO quaternion spline does not implement derivate");
119  }
120 
125  curve_abc_quat_t* compute_derivate_ptr(const std::size_t /*order*/) const {
126  throw std::logic_error(
127  "Compute derivate for quaternion spline is not implemented yet.");
128  }
129 
132  t_waypoint_one_dim_t waypoints;
133  waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));
134  waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones()));
135  return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
136  }
137 
140  virtual std::size_t dim() const { return dim_; }
143  virtual time_t min() const { return min_; }
146  virtual time_t max() const { return max_; }
149  virtual std::size_t degree() const { return 1; }
150 
151  /*Attributes*/
152  Eigen::Quaterniond quat_from_; // const
153  Eigen::Quaterniond quat_to_; // const
154  std::size_t dim_; // const
155  double min_; // const
156  double max_; // const
158  /*Attributes*/
159 }; // End class rotation_spline
160 
161 typedef exact_cubic<Time, Numeric, false, quat_t,
162  std::vector<quat_t, Eigen::aligned_allocator<quat_t> >,
165 
174  /* Constructors - destructors */
175  public:
194  template <typename In>
195  effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
196  quat_ref_const_t& to_quat = quat_t(0, 0, 0, 1),
197  quat_ref_const_t& land_quat = quat_t(0, 0, 0, 1),
198  const Point& lift_normal = Eigen::Vector3d::UnitZ(),
199  const Point& land_normal = Eigen::Vector3d::UnitZ(),
200  const Numeric lift_offset = 0.02,
201  const Numeric land_offset = 0.02,
202  const Time lift_offset_duration = 0.02,
203  const Time land_offset_duration = 0.02)
204  : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal,
205  land_normal, lift_offset, land_offset,
206  lift_offset_duration, land_offset_duration)),
207  to_quat_(to_quat.data()),
208  land_quat_(land_quat.data()),
209  time_lift_offset_(spline_->min() + lift_offset_duration),
210  time_land_offset_(spline_->max() - land_offset_duration),
211  quat_spline_(simple_quat_spline()) {
212  // NOTHING
213  }
214 
237  template <typename In, typename InQuat>
238  effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
239  InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
240  const Point& lift_normal = Eigen::Vector3d::UnitZ(),
241  const Point& land_normal = Eigen::Vector3d::UnitZ(),
242  const Numeric lift_offset = 0.02,
243  const Numeric land_offset = 0.02,
244  const Time lift_offset_duration = 0.02,
245  const Time land_offset_duration = 0.02)
246  : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal,
247  land_normal, lift_offset, land_offset,
248  lift_offset_duration, land_offset_duration)),
249  to_quat_((quatWayPointsBegin->second).data()),
250  land_quat_(((quatWayPointsEnd - 1)->second).data()),
251  time_lift_offset_(spline_->min() + lift_offset_duration),
252  time_land_offset_(spline_->max() - land_offset_duration),
253  quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) {
254  // NOTHING
255  }
256 
257  virtual ~effector_spline_rotation() { delete spline_; }
258  /* Constructors - destructors */
259 
260  /*Helpers*/
261  Numeric min() const { return spline_->min(); }
262  Numeric max() const { return spline_->max(); }
263  /*Helpers*/
264 
265  /*Operations*/
271  config_t operator()(const Numeric t) const {
272  config_t res;
273  res.head<3>() = (*spline_)(t);
274  res.tail<4>() = interpolate_quat(t);
275  return res;
276  }
277 
278  quat_t interpolate_quat(const Numeric t) const {
279  if (t <= time_lift_offset_) return to_quat_.coeffs();
280  if (t >= time_land_offset_) return land_quat_.coeffs();
281  return quat_spline_(t);
282  }
283 
284  private:
285  exact_cubic_quat_t simple_quat_spline() const {
286  std::vector<rotation_spline> splines;
287  splines.push_back(rotation_spline(to_quat_.coeffs(), land_quat_.coeffs(),
289  return exact_cubic_quat_t(splines);
290  }
291 
292  template <typename InQuat>
293  exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin,
294  InQuat quatWayPointsEnd) const {
295  if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) {
296  return simple_quat_spline();
297  }
299  InQuat it(quatWayPointsBegin);
300  Time init = time_lift_offset_;
301  Eigen::Quaterniond current_quat = to_quat_;
302  for (; it != quatWayPointsEnd; ++it) {
303  splines.push_back(
304  rotation_spline(current_quat.coeffs(), it->second, init, it->first));
305  current_quat = it->second;
306  init = it->first;
307  }
308  splines.push_back(rotation_spline(
309  current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
310  return exact_cubic_quat_t(splines);
311  }
312  /*Operations*/
313 
314  public:
315  /*Attributes*/
317  const Eigen::Quaterniond to_quat_;
318  const Eigen::Quaterniond land_quat_;
319  const double time_lift_offset_;
320  const double time_land_offset_;
322  /*Attributes*/
323 }; // End class effector_spline_rotation
324 
325 } // namespace helpers
326 } // namespace ndcurves
327 #endif //_CLASS_EFFECTOR_SPLINE_ROTATION
const Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:31
exact_cubic_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:157
Definition: bernstein.h:20
~rotation_spline()
Definition: effector_spline_rotation.h:55
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:36
effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, InQuat quatWayPointsBegin, InQuat quatWayPointsEnd, const Point &lift_normal=Eigen::Vector3d::UnitZ(), const Point &land_normal=Eigen::Vector3d::UnitZ(), const Numeric lift_offset=0.02, const Numeric land_offset=0.02, const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
Constructor. Given a set of waypoints, and the normal vector of the start and ending positions...
Definition: effector_spline_rotation.h:238
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:321
virtual ~effector_spline_rotation()
Definition: effector_spline_rotation.h:257
virtual bool isApprox(const curve_abc_quat_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: effector_spline_rotation.h:97
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:316
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:32
exact_cubic< Numeric, Numeric, false, point_one_dim_t > exact_cubic_constraint_one_dim
Definition: effector_spline_rotation.h:38
Numeric min() const
Definition: effector_spline_rotation.h:261
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:50
rotation_spline(quat_ref_const_t quat_from=quat_t(0, 0, 0, 1), quat_ref_const_t quat_to=quat_t(0, 0, 0, 1), const double min=0., const double max=1.)
Definition: effector_spline_rotation.h:44
interface for a Curve of arbitrary dimension.
virtual std::size_t dim() const
Get dimension of curve.
Definition: effector_spline_rotation.h:140
virtual std::size_t degree() const
Get the degree of the curve.
Definition: effector_spline_rotation.h:149
curve_abc_quat_t * compute_derivate_ptr(const std::size_t) const
Compute the derived curve at order N.
Definition: effector_spline_rotation.h:125
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:152
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:29
effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, quat_ref_const_t &to_quat=quat_t(0, 0, 0, 1), quat_ref_const_t &land_quat=quat_t(0, 0, 0, 1), const Point &lift_normal=Eigen::Vector3d::UnitZ(), const Point &land_normal=Eigen::Vector3d::UnitZ(), const Numeric lift_offset=0.02, const Numeric land_offset=0.02, const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
Constructor. Given a set of waypoints, and the normal vector of the start and ending positions...
Definition: effector_spline_rotation.h:195
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:278
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:58
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:69
virtual time_t min() const
Get the minimum time for which the curve is defined.
Definition: effector_spline_rotation.h:143
std::size_t dim_
Definition: effector_spline_rotation.h:154
std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:39
double max_
Definition: effector_spline_rotation.h:156
virtual bool operator==(const rotation_spline &other) const
Definition: effector_spline_rotation.h:108
virtual bool operator!=(const rotation_spline &other) const
Definition: effector_spline_rotation.h:112
double Time
Definition: effector_spline.h:27
const double time_land_offset_
Definition: effector_spline_rotation.h:320
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: piecewise_curve.h:647
Represents a trajectory for and end effector. uses the method effector_spline to create a spline traj...
Definition: effector_spline_rotation.h:173
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:116
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:34
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:318
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:40
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:30
Definition: effector_spline_rotation.h:42
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
virtual time_t max() const
Get the maximum time for which the curve is defined.
Definition: effector_spline_rotation.h:146
bool isApprox(const piecewise_curve_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: piecewise_curve.h:112
Numeric max() const
Definition: effector_spline_rotation.h:262
double Numeric
Definition: effector_spline.h:26
virtual Time min() const
Get the minimum time for which the curve is defined.
Definition: piecewise_curve.h:644
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:317
exact_cubic_t * effector_spline(In wayPointsBegin, In wayPointsEnd, const Point &lift_normal=Eigen::Vector3d::UnitZ(), const Point &land_normal=Eigen::Vector3d::UnitZ(), const Numeric lift_offset=0.02, const Numeric land_offset=0.02, const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
Helper method to create a spline typically used to guide the 3d trajectory of a robot end effector...
Definition: effector_spline.h:97
exact_cubic< Time, Numeric, false, quat_t, std::vector< quat_t, Eigen::aligned_allocator< quat_t > >, rotation_spline > exact_cubic_quat_t
Definition: effector_spline_rotation.h:164
curve_abc< Time, Numeric, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:33
const double time_lift_offset_
Definition: effector_spline_rotation.h:319
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:153
Time time_t
Definition: curve_abc.h:40
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:271
double min_
Definition: effector_spline_rotation.h:155
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:35
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37
bool isApprox(const rotation_spline &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: effector_spline_rotation.h:87
exact_cubic_constraint_one_dim computeWayPoints() const
Initialize time reparametrization for spline.
Definition: effector_spline_rotation.h:131