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