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 
23 #include "ndcurves/curve_abc.h"
24 #include <Eigen/Geometry>
25 
26 namespace ndcurves {
27 namespace helpers {
28 typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
29 typedef Eigen::Ref<quat_t> quat_ref_t;
30 typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
31 typedef Eigen::Matrix<Numeric, 7, 1> config_t;
33 typedef std::pair<Numeric, quat_t> waypoint_quat_t;
34 typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
35 typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
37 typedef std::pair<Numeric, point_one_dim_t> waypoint_one_dim_t;
38 typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
39 
40 class rotation_spline : public curve_abc_quat_t {
41  public:
42  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),
43  const double min = 0., const double max = 1.)
44  : curve_abc_quat_t(),
45  quat_from_(quat_from.data()),
46  quat_to_(quat_to.data()),
47  dim_(4),
48  min_(min),
49  max_(max),
51 
53 
54  /* Copy Constructors / operator=*/
56  quat_from_ = from.quat_from_;
57  quat_to_ = from.quat_to_;
58  dim_ = from.dim_;
59  min_ = from.min_;
60  max_ = from.max_;
62  return *this;
63  }
64  /* Copy Constructors / operator=*/
65 
66  quat_t operator()(const Numeric t) const {
67  if (t <= min()) return quat_from_.coeffs();
68  if (t >= max()) return quat_to_.coeffs();
69  // normalize u
70  Numeric u = (t - min()) / (max() - min());
71  // reparametrize u
72  return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
73  }
74 
83  bool isApprox(const rotation_spline& other,
84  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
85  return ndcurves::isApprox<Numeric>(min_, other.min_) && ndcurves::isApprox<Numeric>(max_, other.max_) &&
86  dim_ == other.dim_ && quat_from_.isApprox(other.quat_from_, prec) &&
87  quat_to_.isApprox(other.quat_to_, prec) && time_reparam_.isApprox(other.time_reparam_, prec);
88  }
89 
90  virtual bool isApprox(const curve_abc_quat_t* other,
91  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
92  const rotation_spline* other_cast = dynamic_cast<const rotation_spline*>(other);
93  if (other_cast)
94  return isApprox(*other_cast, prec);
95  else
96  return false;
97  }
98 
99  virtual bool operator==(const rotation_spline& other) const { return isApprox(other); }
100 
101  virtual bool operator!=(const rotation_spline& other) const { return !(*this == other); }
102 
103  virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const {
104  throw std::runtime_error("TODO quaternion spline does not implement derivate");
105  }
106 
110  curve_abc_quat_t* compute_derivate_ptr(const std::size_t /*order*/) const {
111  throw std::logic_error("Compute derivate for quaternion spline is not implemented yet.");
112  }
113 
115  exact_cubic_constraint_one_dim computeWayPoints() const {
116  t_waypoint_one_dim_t waypoints;
117  waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));
118  waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones()));
119  return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
120  }
121 
124  virtual std::size_t dim() const { return dim_; }
127  virtual time_t min() const { return min_; }
130  virtual time_t max() const { return max_; }
133  virtual std::size_t degree() const { return 1; }
134 
135  /*Attributes*/
136  Eigen::Quaterniond quat_from_; // const
137  Eigen::Quaterniond quat_to_; // const
138  std::size_t dim_; // const
139  double min_; // const
140  double max_; // const
141  exact_cubic_constraint_one_dim time_reparam_; // const
142  /*Attributes*/
143 }; // End class rotation_spline
144 
148 
157  /* Constructors - destructors */
158  public:
175  template <typename In>
176  effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, quat_ref_const_t& to_quat = quat_t(0, 0, 0, 1),
177  quat_ref_const_t& land_quat = quat_t(0, 0, 0, 1),
178  const Point& lift_normal = Eigen::Vector3d::UnitZ(),
179  const Point& land_normal = Eigen::Vector3d::UnitZ(), const Numeric lift_offset = 0.02,
180  const Numeric land_offset = 0.02, const Time lift_offset_duration = 0.02,
181  const Time land_offset_duration = 0.02)
182  : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
183  lift_offset_duration, land_offset_duration)),
184  to_quat_(to_quat.data()),
185  land_quat_(land_quat.data()),
186  time_lift_offset_(spline_->min() + lift_offset_duration),
187  time_land_offset_(spline_->max() - land_offset_duration),
188  quat_spline_(simple_quat_spline()) {
189  // NOTHING
190  }
191 
210  template <typename In, typename InQuat>
211  effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
212  const Point& lift_normal = Eigen::Vector3d::UnitZ(),
213  const Point& land_normal = Eigen::Vector3d::UnitZ(), const Numeric lift_offset = 0.02,
214  const Numeric land_offset = 0.02, const Time lift_offset_duration = 0.02,
215  const Time land_offset_duration = 0.02)
216  : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
217  lift_offset_duration, land_offset_duration)),
218  to_quat_((quatWayPointsBegin->second).data()),
219  land_quat_(((quatWayPointsEnd - 1)->second).data()),
220  time_lift_offset_(spline_->min() + lift_offset_duration),
221  time_land_offset_(spline_->max() - land_offset_duration),
222  quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) {
223  // NOTHING
224  }
225 
226  virtual ~effector_spline_rotation() { delete spline_; }
227  /* Constructors - destructors */
228 
229  /*Helpers*/
230  Numeric min() const { return spline_->min(); }
231  Numeric max() const { return spline_->max(); }
232  /*Helpers*/
233 
234  /*Operations*/
240  config_t operator()(const Numeric t) const {
241  config_t res;
242  res.head<3>() = (*spline_)(t);
243  res.tail<4>() = interpolate_quat(t);
244  return res;
245  }
246 
247  quat_t interpolate_quat(const Numeric t) const {
248  if (t <= time_lift_offset_) return to_quat_.coeffs();
249  if (t >= time_land_offset_) return land_quat_.coeffs();
250  return quat_spline_(t);
251  }
252 
253  private:
254  exact_cubic_quat_t simple_quat_spline() const {
255  std::vector<rotation_spline> splines;
256  splines.push_back(rotation_spline(to_quat_.coeffs(), land_quat_.coeffs(), time_lift_offset_, time_land_offset_));
257  return exact_cubic_quat_t(splines);
258  }
259 
260  template <typename InQuat>
261  exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const {
262  if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) {
263  return simple_quat_spline();
264  }
266  InQuat it(quatWayPointsBegin);
267  Time init = time_lift_offset_;
268  Eigen::Quaterniond current_quat = to_quat_;
269  for (; it != quatWayPointsEnd; ++it) {
270  splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
271  current_quat = it->second;
272  init = it->first;
273  }
274  splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
275  return exact_cubic_quat_t(splines);
276  }
277  /*Operations*/
278 
279  public:
280  /*Attributes*/
282  const Eigen::Quaterniond to_quat_;
283  const Eigen::Quaterniond land_quat_;
284  const double time_lift_offset_;
285  const double time_land_offset_;
287  /*Attributes*/
288 }; // End class effector_spline_rotation
289 
290 } // namespace helpers
291 } // namespace ndcurves
292 #endif //_CLASS_EFFECTOR_SPLINE_ROTATION
const Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:30
exact_cubic_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:141
Definition: bernstein.h:20
~rotation_spline()
Definition: effector_spline_rotation.h:52
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:35
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:211
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:286
virtual ~effector_spline_rotation()
Definition: effector_spline_rotation.h:226
virtual bool isApprox(const curve_abc_quat_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: effector_spline_rotation.h:90
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:281
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:31
exact_cubic< Numeric, Numeric, false, point_one_dim_t > exact_cubic_constraint_one_dim
Definition: effector_spline_rotation.h:36
Numeric min() const
Definition: effector_spline_rotation.h:230
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:42
interface for a Curve of arbitrary dimension.
virtual std::size_t dim() const
Get dimension of curve.
Definition: effector_spline_rotation.h:124
virtual std::size_t degree() const
Get the degree of the curve.
Definition: effector_spline_rotation.h:133
curve_abc_quat_t * compute_derivate_ptr(const std::size_t) const
Compute the derived curve at order N.
Definition: effector_spline_rotation.h:110
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:136
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:28
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:176
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:247
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:55
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:66
virtual time_t min() const
Get the minimum time for which the curve is defined.
Definition: effector_spline_rotation.h:127
std::size_t dim_
Definition: effector_spline_rotation.h:138
std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:37
double max_
Definition: effector_spline_rotation.h:140
virtual bool operator==(const rotation_spline &other) const
Definition: effector_spline_rotation.h:99
virtual bool operator!=(const rotation_spline &other) const
Definition: effector_spline_rotation.h:101
double Time
Definition: effector_spline.h:27
const double time_land_offset_
Definition: effector_spline_rotation.h:285
Represents a trajectory for and end effector. uses the method effector_spline to create a spline traj...
Definition: effector_spline_rotation.h:156
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:103
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:33
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:283
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:38
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:29
Definition: effector_spline_rotation.h:40
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:130
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:92
Numeric max() const
Definition: effector_spline_rotation.h:231
double Numeric
Definition: effector_spline.h:26
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:282
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:91
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:147
curve_abc< Time, Numeric, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:32
const double time_lift_offset_
Definition: effector_spline_rotation.h:284
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:137
Time time_t
Definition: curve_abc.h:37
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:240
double min_
Definition: effector_spline_rotation.h:139
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:34
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
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:83
exact_cubic_constraint_one_dim computeWayPoints() const
Initialize time reparametrization for spline.
Definition: effector_spline_rotation.h:115