effector_spline_rotation.h 12.9 KB
Newer Older
1
/**
Guilhem Saurel's avatar
Guilhem Saurel committed
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
 * \file exact_cubic.h
 * \brief class allowing to create an Exact cubic spline.
 * \author Steve T.
 * \version 0.1
 * \date 06/17/2013
 *
 * This file contains definitions for the ExactCubic class.
 * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
 * cubic splines fulfulling those 4 restrictions :
 * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
 * - x_i(t_i+1) = x_i+1* ;
 * - its derivative is continous at t_i+1
 * - its acceleration is continous at t_i+1
 * more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
 * By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
 */
18
19
20
21

#ifndef _CLASS_EFFECTOR_SPLINE_ROTATION
#define _CLASS_EFFECTOR_SPLINE_ROTATION

22
23
#include "curves/helpers/effector_spline.h"
#include "curves/curve_abc.h"
24
25
#include <Eigen/Geometry>

Guilhem Saurel's avatar
Guilhem Saurel committed
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
namespace curves {
namespace helpers {
typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
typedef Eigen::Ref<quat_t> quat_ref_t;
typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
typedef Eigen::Matrix<Numeric, 7, 1> config_t;
typedef curve_abc<Time, Numeric, false, quat_t> curve_abc_quat_t;
typedef std::pair<Numeric, quat_t> waypoint_quat_t;
typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
typedef exact_cubic<Numeric, Numeric, false, point_one_dim_t> exact_cubic_constraint_one_dim;
typedef std::pair<Numeric, point_one_dim_t> waypoint_one_dim_t;
typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;

class rotation_spline : public curve_abc_quat_t {
 public:
  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.)
      : curve_abc_quat_t(),
        quat_from_(quat_from.data()),
        quat_to_(quat_to.data()),
47
        dim_(4),
Guilhem Saurel's avatar
Guilhem Saurel committed
48
49
50
51
52
53
54
55
56
57
        min_(min),
        max_(max),
        time_reparam_(computeWayPoints()) {}

  ~rotation_spline() {}

  /* Copy Constructors / operator=*/
  rotation_spline& operator=(const rotation_spline& from) {
    quat_from_ = from.quat_from_;
    quat_to_ = from.quat_to_;
58
    dim_ = from.dim_;
Guilhem Saurel's avatar
Guilhem Saurel committed
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
    min_ = from.min_;
    max_ = from.max_;
    time_reparam_ = exact_cubic_constraint_one_dim(from.time_reparam_);
    return *this;
  }
  /* Copy Constructors / operator=*/

  quat_t operator()(const Numeric t) const {
    if (t <= min()) return quat_from_.coeffs();
    if (t >= max()) return quat_to_.coeffs();
    // normalize u
    Numeric u = (t - min()) / (max() - min());
    // reparametrize u
    return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
  }

75
  /**
76
77
   * @brief isApprox check if other and *this are approximately equals.
   * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
78
79
80
81
   * @param other the other curve to check
   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
   * @return true is the two curves are approximately equals
   */
82
  bool isApprox(const rotation_spline& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
83
84
    return curves::isApprox<Numeric> (min_, other.min_)
        && curves::isApprox<Numeric> (max_, other.max_)
85
86
87
        && dim_ == other.dim_
        && quat_from_.isApprox(other.quat_from_,prec)
        && quat_to_.isApprox(other.quat_to_,prec)
88
89
90
91
92
93
94
95
96
        && time_reparam_.isApprox(other.time_reparam_,prec);
  }

  virtual bool isApprox(const curve_abc_quat_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
    const rotation_spline* other_cast = dynamic_cast<const rotation_spline*>(other);
    if(other_cast)
      return isApprox(*other_cast,prec);
    else
      return false;
97
98
99
100
101
102
103
104
105
106
107
  }

  virtual bool operator==(const rotation_spline& other) const {
    return isApprox(other);
  }

  virtual bool operator!=(const rotation_spline& other) const {
    return !(*this == other);
  }


Guilhem Saurel's avatar
Guilhem Saurel committed
108
109
110
111
  virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const {
    throw std::runtime_error("TODO quaternion spline does not implement derivate");
  }

112
113
114
115
  ///  \brief Compute the derived curve at order N.
  ///  \param order : order of derivative.
  ///  \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
  curve_abc_quat_t* compute_derivate_ptr(const std::size_t /*order*/) const {
116
117
118
    throw std::logic_error("Compute derivate for quaternion spline is not implemented yet.");
  }

119

Guilhem Saurel's avatar
Guilhem Saurel committed
120
121
122
123
124
125
126
127
128
129
130
  /// \brief Initialize time reparametrization for spline.
  exact_cubic_constraint_one_dim computeWayPoints() const {
    t_waypoint_one_dim_t waypoints;
    waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));
    waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones()));
    return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
  }

  virtual std::size_t dim() const { return dim_; }
  virtual time_t min() const { return min_; }
  virtual time_t max() const { return max_; }
131
132
133
  /// \brief Get the degree of the curve.
  /// \return \f$degree\f$, the degree of the curve.
  virtual std::size_t  degree() const {return 1;}
Guilhem Saurel's avatar
Guilhem Saurel committed
134
135
136
137

  /*Attributes*/
  Eigen::Quaterniond quat_from_;                 // const
  Eigen::Quaterniond quat_to_;                   // const
138
  std::size_t dim_;                              // const
Guilhem Saurel's avatar
Guilhem Saurel committed
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
  double min_;                                   // const
  double max_;                                   // const
  exact_cubic_constraint_one_dim time_reparam_;  // const
  /*Attributes*/
};  // End class rotation_spline

typedef exact_cubic<Time, Numeric, false, quat_t, std::vector<quat_t, Eigen::aligned_allocator<quat_t> >,
                    rotation_spline>
    exact_cubic_quat_t;

/// \class effector_spline_rotation.
/// \brief Represents a trajectory for and end effector.
/// uses the method effector_spline to create a spline trajectory.
/// Additionally, handles the rotation of the effector as follows:
/// does not rotate during the take off and landing phase,
/// then uses a SLERP algorithm to interpolate the rotation in the quaternion
/// space.
class effector_spline_rotation {
  /* Constructors - destructors */
 public:
  /// \brief Constructor.
  /// Given a set of waypoints, and the normal vector of the start and
  /// ending positions, automatically create the spline such that:
  /// + init and end velocities / accelerations are 0
  /// + the effector lifts and lands exactly in the direction of the specified normals.
  /// \param wayPointsBegin   : an iterator pointing to the first element of a waypoint container.
  /// \param wayPointsEnd     : an iterator pointing to the last element of a waypoint container.
  /// \param to_quat          : 4D vector, quaternion indicating rotation at take off(x, y, z, w).
  /// \param land_quat        : 4D vector, quaternion indicating rotation at landing (x, y, z, w).
  /// \param lift_normal      : normal to be followed by end effector at take-off.
  /// \param land_normal      : normal to be followed by end effector at landing.
  /// \param lift_offset      : length of the straight line along normal at take-off.
  /// \param land_offset      : length of the straight line along normal at landing.
  /// \param lift_offset_duration : time travelled along straight line at take-off.
  /// \param land_offset_duration : time travelled along straight line at landing.
  ///
  template <typename In>
  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)
      : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
                                lift_offset_duration, land_offset_duration)),
        to_quat_(to_quat.data()),
        land_quat_(land_quat.data()),
        time_lift_offset_(spline_->min() + lift_offset_duration),
        time_land_offset_(spline_->max() - land_offset_duration),
        quat_spline_(simple_quat_spline()) {
    // NOTHING
  }

  /// \brief Constructor.
  /// Given a set of waypoints, and the normal vector of the start and
  /// ending positions, automatically create the spline such that:
  /// + init and end velocities / accelerations are 0
  /// + the effector lifts and lands exactly in the direction of the specified normals.
  /// \param wayPointsBegin       : an iterator pointing to the first element of a waypoint container.
  /// \param wayPointsEnd         : an iterator pointing to the last element of a waypoint container.
  /// \param quatWayPointsBegin   : en iterator pointing to the first element of a 4D vector (x, y, z, w) container of
  ///  quaternions indicating rotation at specific time steps.
  /// \param quatWayPointsEnd     : en iterator pointing to the last element of a 4D vector (x, y, z, w) container of
  ///  quaternions indicating rotation at specific time steps.
  /// \param lift_normal          : normal to be followed by end effector at take-off.
  /// \param land_normal          : normal to be followed by end effector at landing.
  /// \param lift_offset          : length of the straight line along normal at take-off.
  /// \param land_offset          : length of the straight line along normal at landing.
  /// \param lift_offset_duration : time travelled along straight line at take-off.
  /// \param land_offset_duration : time travelled along straight line at landing.
  ///
  template <typename In, typename InQuat>
  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)
      : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
                                lift_offset_duration, land_offset_duration)),
        to_quat_((quatWayPointsBegin->second).data()),
        land_quat_(((quatWayPointsEnd - 1)->second).data()),
        time_lift_offset_(spline_->min() + lift_offset_duration),
        time_land_offset_(spline_->max() - land_offset_duration),
        quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) {
    // NOTHING
  }

  ~effector_spline_rotation() { delete spline_; }
  /* Constructors - destructors */

  /*Helpers*/
  Numeric min() const { return spline_->min(); }
  Numeric max() const { return spline_->max(); }
  /*Helpers*/

  /*Operations*/
  ///  \brief Evaluation of the effector position and rotation at time t.
  ///  \param t : the time when to evaluate the spline.
  ///  \return A 7D vector where the 3 first values are the 3D position and the 4 last are the
  ///  quaternion describing the rotation.
  ///
  config_t operator()(const Numeric t) const {
    config_t res;
    res.head<3>() = (*spline_)(t);
    res.tail<4>() = interpolate_quat(t);
    return res;
  }

  quat_t interpolate_quat(const Numeric t) const {
    if (t <= time_lift_offset_) return to_quat_.coeffs();
    if (t >= time_land_offset_) return land_quat_.coeffs();
    return quat_spline_(t);
  }

 private:
  exact_cubic_quat_t simple_quat_spline() const {
    std::vector<rotation_spline> splines;
    splines.push_back(rotation_spline(to_quat_.coeffs(), land_quat_.coeffs(), time_lift_offset_, time_land_offset_));
    return exact_cubic_quat_t(splines);
  }

  template <typename InQuat>
  exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const {
    if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) {
      return simple_quat_spline();
    }
    exact_cubic_quat_t::t_spline_t splines;
    InQuat it(quatWayPointsBegin);
    Time init = time_lift_offset_;
    Eigen::Quaterniond current_quat = to_quat_;
    for (; it != quatWayPointsEnd; ++it) {
      splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
      current_quat = it->second;
      init = it->first;
    }
    splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
    return exact_cubic_quat_t(splines);
  }
  /*Operations*/

 public:
  /*Attributes*/
  const exact_cubic_t* spline_;
  const Eigen::Quaterniond to_quat_;
  const Eigen::Quaterniond land_quat_;
  const double time_lift_offset_;
  const double time_land_offset_;
  const exact_cubic_quat_t quat_spline_;
  /*Attributes*/
};  // End class effector_spline_rotation

}  // namespace helpers
}  // namespace curves
#endif  //_CLASS_EFFECTOR_SPLINE_ROTATION