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 "spline/helpers/effector_spline.h"
23 #include "spline/curve_abc.h"
24 #include <Eigen/Geometry>
25 
26 namespace spline {
27 namespace helpers {
28 
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;
33 typedef curve_abc<Time, Numeric, 4, false, quat_t> curve_abc_quat_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;
37 typedef spline_deriv_constraint<Numeric, Numeric, 1, false, point_one_dim_t> spline_deriv_constraint_one_dim;
38 typedef std::pair<Numeric, point_one_dim_t> waypoint_one_dim_t;
39 typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
40 
42  public:
43  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),
44  const double min = 0., const double max = 1.)
45  : curve_abc_quat_t(),
46  quat_from_(quat_from.data()),
47  quat_to_(quat_to.data()),
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  min_ = from.min_;
59  max_ = from.max_;
61  }
62  /* Copy Constructors / operator=*/
63 
64  quat_t operator()(const Numeric t) const {
65  if (t <= min()) return quat_from_.coeffs();
66  if (t >= max()) return quat_to_.coeffs();
67  // normalize u
68  Numeric u = (t - min()) / (max() - min());
69  // reparametrize u
70  return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
71  }
72 
73  virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const {
74  throw std::runtime_error("TODO quaternion spline does not implement derivate");
75  }
76 
78  // initializing time reparametrization for spline
79  t_waypoint_one_dim_t waypoints;
80  waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));
81  waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones()));
82  return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
83  }
84 
85  virtual time_t min() const { return min_; }
86  virtual time_t max() const { return max_; }
87 
88  public:
89  Eigen::Quaterniond quat_from_; // const
90  Eigen::Quaterniond quat_to_; // const
91  double min_; // const
92  double max_; // const
94 };
95 
96 typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t, Eigen::aligned_allocator<quat_t> >,
99 
108  /* Constructors - destructors */
109  public:
126  template <typename In>
127  effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, quat_ref_const_t& to_quat = quat_t(0, 0, 0, 1),
128  quat_ref_const_t& land_quat = quat_t(0, 0, 0, 1),
129  const Point& lift_normal = Eigen::Vector3d::UnitZ(),
130  const Point& land_normal = Eigen::Vector3d::UnitZ(), const Numeric lift_offset = 0.02,
131  const Numeric land_offset = 0.02, const Time lift_offset_duration = 0.02,
132  const Time land_offset_duration = 0.02)
133  : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
134  lift_offset_duration, land_offset_duration)),
135  to_quat_(to_quat.data()),
136  land_quat_(land_quat.data()),
137  time_lift_offset_(spline_->min() + lift_offset_duration),
138  time_land_offset_(spline_->max() - land_offset_duration),
139  quat_spline_(simple_quat_spline()) {
140  // NOTHING
141  }
142 
161  template <typename In, typename InQuat>
162  effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
163  const Point& lift_normal = Eigen::Vector3d::UnitZ(),
164  const Point& land_normal = Eigen::Vector3d::UnitZ(), const Numeric lift_offset = 0.02,
165  const Numeric land_offset = 0.02, const Time lift_offset_duration = 0.02,
166  const Time land_offset_duration = 0.02)
167  : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
168  lift_offset_duration, land_offset_duration)),
169  to_quat_((quatWayPointsBegin->second).data()),
170  land_quat_(((quatWayPointsEnd - 1)->second).data()),
171  time_lift_offset_(spline_->min() + lift_offset_duration),
172  time_land_offset_(spline_->max() - land_offset_duration),
173  quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) {
174  // NOTHING
175  }
176 
178  /* Constructors - destructors */
179 
180  /*Helpers*/
181  public:
182  Numeric min() const { return spline_->min(); }
183  Numeric max() const { return spline_->max(); }
184  /*Helpers*/
185 
186  /*Operations*/
187  public:
193  config_t operator()(const Numeric t) const {
194  config_t res;
195  res.head<3>() = (*spline_)(t);
196  res.tail<4>() = interpolate_quat(t);
197  return res;
198  }
199 
200  public:
201  quat_t interpolate_quat(const Numeric t) const {
202  if (t <= time_lift_offset_) return to_quat_.coeffs();
203  if (t >= time_land_offset_) return land_quat_.coeffs();
204  return quat_spline_(t);
205  }
206 
207  private:
208  exact_cubic_quat_t simple_quat_spline() const {
209  std::vector<rotation_spline> splines;
210  splines.push_back(rotation_spline(to_quat_.coeffs(), land_quat_.coeffs(), time_lift_offset_, time_land_offset_));
211  return exact_cubic_quat_t(splines);
212  }
213 
214  template <typename InQuat>
215  exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const {
216  if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) return simple_quat_spline();
218  InQuat it(quatWayPointsBegin);
219  Time init = time_lift_offset_;
220  Eigen::Quaterniond current_quat = to_quat_;
221  for (; it != quatWayPointsEnd; ++it) {
222  splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
223  current_quat = it->second;
224  init = it->first;
225  }
226  splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
227  return exact_cubic_quat_t(splines);
228  }
229 
230  /*Operations*/
231 
232  /*Attributes*/
233  public:
235  const Eigen::Quaterniond to_quat_;
236  const Eigen::Quaterniond land_quat_;
237  const double time_lift_offset_;
238  const double time_land_offset_;
240  /*Attributes*/
241 };
242 
243 } // namespace helpers
244 } // namespace spline
245 #endif //_CLASS_EFFECTOR_SPLINE_ROTATION
curve_abc< Time, Numeric, 4, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:33
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:36
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:73
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:39
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:90
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:239
exact_cubic< Time, Numeric, 4, false, quat_t, std::vector< quat_t, Eigen::aligned_allocator< quat_t > >, rotation_spline > exact_cubic_quat_t
Definition: effector_spline_rotation.h:98
Definition: bernstein.h:20
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:30
~effector_spline_rotation()
Definition: effector_spline_rotation.h:177
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:35
Represents a trajectory for and end effector uses the method effector_spline to create a spline traje...
Definition: effector_spline_rotation.h:107
Numeric min() const
Definition: effector_spline_rotation.h:182
const Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:31
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:193
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:127
const double time_lift_offset_
Definition: effector_spline_rotation.h:237
double min_
Definition: effector_spline_rotation.h:91
double Time
Definition: effector_spline.h:27
double Numeric
Definition: effector_spline.h:26
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:34
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:235
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:234
const double time_land_offset_
Definition: effector_spline_rotation.h:238
std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:38
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:89
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:43
virtual time_t max() const
Definition: effector_spline_rotation.h:86
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:32
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:90
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:64
Definition: effector_spline_rotation.h:41
spline_deriv_constraint_one_dim computeWayPoints() const
Definition: effector_spline_rotation.h:77
spline_deriv_constraint< Numeric, Numeric, 1, false, point_one_dim_t > spline_deriv_constraint_one_dim
Definition: effector_spline_rotation.h:37
~rotation_spline()
Definition: effector_spline_rotation.h:52
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:162
spline_deriv_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:93
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:236
Numeric max() const
Definition: effector_spline_rotation.h:183
double max_
Definition: effector_spline_rotation.h:92
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:29
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition: effector_spline.h:34
virtual time_t min() const
Definition: effector_spline_rotation.h:85
spline_deriv_constraint_t::t_spline_t t_spline_t
Definition: effector_spline.h:35
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:55
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:201
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28