| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | * \file exact_cubic.h | ||
| 3 | * \brief class allowing to create an Exact cubic spline. | ||
| 4 | * \author Steve T. | ||
| 5 | * \version 0.1 | ||
| 6 | * \date 06/17/2013 | ||
| 7 | * | ||
| 8 | * This file contains definitions for the ExactCubic class. | ||
| 9 | * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique | ||
| 10 | * set of cubic splines fulfulling those 4 restrictions : | ||
| 11 | * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint | ||
| 12 | * - x_i(t_i+1) = x_i+1* ; | ||
| 13 | * - its derivative is continous at t_i+1 | ||
| 14 | * - its acceleration is continous at t_i+1 | ||
| 15 | * more details in paper "Task-Space Trajectories via Cubic Spline Optimization" | ||
| 16 | * By J. Zico Kolter and Andrew Y.ng (ICRA 2009) | ||
| 17 | */ | ||
| 18 | |||
| 19 | #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION | ||
| 20 | #define _CLASS_EFFECTOR_SPLINE_ROTATION | ||
| 21 | |||
| 22 | #include <Eigen/Geometry> | ||
| 23 | |||
| 24 | #include "ndcurves/curve_abc.h" | ||
| 25 | #include "ndcurves/helpers/effector_spline.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; | ||
| 33 | typedef curve_abc<Time, Numeric, 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 exact_cubic<Numeric, Numeric, false, point_one_dim_t> | ||
| 38 | exact_cubic_constraint_one_dim; | ||
| 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 | |||
| 42 | class rotation_spline : public curve_abc_quat_t { | ||
| 43 | public: | ||
| 44 | 7 | 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 | 7 | : curve_abc_quat_t(), | |
| 48 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | quat_from_(quat_from.data()), |
| 49 |
1/2✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
|
7 | quat_to_(quat_to.data()), |
| 50 | 7 | dim_(4), | |
| 51 | 7 | min_(min), | |
| 52 | 7 | max_(max), | |
| 53 |
1/2✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
14 | time_reparam_(computeWayPoints()) {} |
| 54 | |||
| 55 | 44 | ~rotation_spline() {} | |
| 56 | |||
| 57 | /* Copy Constructors / operator=*/ | ||
| 58 | rotation_spline& operator=(const rotation_spline& from) { | ||
| 59 | quat_from_ = from.quat_from_; | ||
| 60 | quat_to_ = from.quat_to_; | ||
| 61 | dim_ = from.dim_; | ||
| 62 | min_ = from.min_; | ||
| 63 | max_ = from.max_; | ||
| 64 | time_reparam_ = exact_cubic_constraint_one_dim(from.time_reparam_); | ||
| 65 | return *this; | ||
| 66 | } | ||
| 67 | /* Copy Constructors / operator=*/ | ||
| 68 | |||
| 69 | 3 | quat_t operator()(const Numeric t) const { | |
| 70 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2 times.
|
3 | if (t <= min()) return quat_from_.coeffs(); |
| 71 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
2 | if (t >= max()) return quat_to_.coeffs(); |
| 72 | // normalize u | ||
| 73 | 2 | Numeric u = (t - min()) / (max() - min()); | |
| 74 | // re-parametrize u | ||
| 75 |
4/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
|
2 | return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs(); |
| 76 | } | ||
| 77 | |||
| 78 | /** | ||
| 79 | * @brief isApprox check if other and *this are approximately equals. | ||
| 80 | * Only two curves of the same class can be approximately equals, for | ||
| 81 | * comparison between different type of curves see isEquivalent | ||
| 82 | * @param other the other curve to check | ||
| 83 | * @param prec the precision threshold, default | ||
| 84 | * Eigen::NumTraits<Numeric>::dummy_precision() | ||
| 85 | * @return true is the two curves are approximately equals | ||
| 86 | */ | ||
| 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) && | |
| 94 | ✗ | time_reparam_.isApprox(other.time_reparam_, 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 | |||
| 121 | /// \brief Compute the derived curve at order N. | ||
| 122 | /// \param order : order of derivative. | ||
| 123 | /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the | ||
| 124 | /// curve. | ||
| 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 | |||
| 130 | /// \brief Initialize time re-parametrization for spline. | ||
| 131 | 7 | exact_cubic_constraint_one_dim computeWayPoints() const { | |
| 132 | 7 | t_waypoint_one_dim_t waypoints; | |
| 133 |
4/8✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
|
7 | waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero())); |
| 134 |
4/8✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
|
7 | waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones())); |
| 135 |
1/2✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
14 | return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end()); |
| 136 | 7 | } | |
| 137 | |||
| 138 | /// \brief Get dimension of curve. | ||
| 139 | /// \return dimension of curve. | ||
| 140 | 9 | virtual std::size_t dim() const { return dim_; } | |
| 141 | /// \brief Get the minimum time for which the curve is defined. | ||
| 142 | /// \return \f$t_{min}\f$, lower bound of time range. | ||
| 143 | 16 | virtual time_t min() const { return min_; } | |
| 144 | /// \brief Get the maximum time for which the curve is defined. | ||
| 145 | /// \return \f$t_{max}\f$, upper bound of time range. | ||
| 146 | 10 | virtual time_t max() const { return max_; } | |
| 147 | /// \brief Get the degree of the curve. | ||
| 148 | /// \return \f$degree\f$, the degree of the curve. | ||
| 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 | ||
| 157 | exact_cubic_constraint_one_dim time_reparam_; // 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> >, | ||
| 163 | rotation_spline> | ||
| 164 | exact_cubic_quat_t; | ||
| 165 | |||
| 166 | /// \class effector_spline_rotation. | ||
| 167 | /// \brief Represents a trajectory for and end effector. | ||
| 168 | /// uses the method effector_spline to create a spline trajectory. | ||
| 169 | /// Additionally, handles the rotation of the effector as follows: | ||
| 170 | /// does not rotate during the take off and landing phase, | ||
| 171 | /// then uses a SLERP algorithm to interpolate the rotation in the quaternion | ||
| 172 | /// space. | ||
| 173 | class effector_spline_rotation { | ||
| 174 | /* Constructors - destructors */ | ||
| 175 | public: | ||
| 176 | /// \brief Constructor. | ||
| 177 | /// Given a set of waypoints, and the normal vector of the start and | ||
| 178 | /// ending positions, automatically create the spline such that: | ||
| 179 | /// + init and end velocities / accelerations are 0 | ||
| 180 | /// + the effector lifts and lands exactly in the direction of the specified | ||
| 181 | /// normals. \param wayPointsBegin : an iterator pointing to the first | ||
| 182 | /// element of a waypoint container. \param wayPointsEnd : an iterator | ||
| 183 | /// pointing to the last element of a waypoint container. \param to_quat : 4D | ||
| 184 | /// vector, quaternion indicating rotation at take off(x, y, z, w). \param | ||
| 185 | /// land_quat : 4D vector, quaternion indicating rotation at landing | ||
| 186 | /// (x, y, z, w). \param lift_normal : normal to be followed by end | ||
| 187 | /// effector at take-off. \param land_normal : normal to be followed by | ||
| 188 | /// end effector at landing. \param lift_offset : length of the straight | ||
| 189 | /// line along normal at take-off. \param land_offset : length of the | ||
| 190 | /// straight line along normal at landing. \param lift_offset_duration : time | ||
| 191 | /// travelled along straight line at take-off. \param land_offset_duration : | ||
| 192 | /// time travelled along straight line at landing. | ||
| 193 | /// | ||
| 194 | template <typename In> | ||
| 195 | 2 | 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 | 4 | : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, | |
| 205 | land_normal, lift_offset, land_offset, | ||
| 206 | lift_offset_duration, land_offset_duration)), | ||
| 207 | 2 | to_quat_(to_quat.data()), | |
| 208 | 2 | land_quat_(land_quat.data()), | |
| 209 | 2 | time_lift_offset_(spline_->min() + lift_offset_duration), | |
| 210 | 2 | time_land_offset_(spline_->max() - land_offset_duration), | |
| 211 | 4 | quat_spline_(simple_quat_spline()) { | |
| 212 | // NOTHING | ||
| 213 | 2 | } | |
| 214 | |||
| 215 | /// \brief Constructor. | ||
| 216 | /// Given a set of waypoints, and the normal vector of the start and | ||
| 217 | /// ending positions, automatically create the spline such that: | ||
| 218 | /// + init and end velocities / accelerations are 0 | ||
| 219 | /// + the effector lifts and lands exactly in the direction of the specified | ||
| 220 | /// normals. \param wayPointsBegin : an iterator pointing to the first | ||
| 221 | /// element of a waypoint container. \param wayPointsEnd : an iterator | ||
| 222 | /// pointing to the last element of a waypoint container. \param | ||
| 223 | /// quatWayPointsBegin : en iterator pointing to the first element of a 4D | ||
| 224 | /// vector (x, y, z, w) container of | ||
| 225 | /// quaternions indicating rotation at specific time steps. | ||
| 226 | /// \param quatWayPointsEnd : en iterator pointing to the last element of | ||
| 227 | /// a 4D vector (x, y, z, w) container of | ||
| 228 | /// quaternions indicating rotation at specific time steps. | ||
| 229 | /// \param lift_normal : normal to be followed by end effector at | ||
| 230 | /// take-off. \param land_normal : normal to be followed by end | ||
| 231 | /// effector at landing. \param lift_offset : length of the straight | ||
| 232 | /// line along normal at take-off. \param land_offset : length of the | ||
| 233 | /// straight line along normal at landing. \param lift_offset_duration : time | ||
| 234 | /// travelled along straight line at take-off. \param land_offset_duration : | ||
| 235 | /// time travelled along straight line at landing. | ||
| 236 | /// | ||
| 237 | template <typename In, typename InQuat> | ||
| 238 | 1 | 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 | 2 | : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, | |
| 247 | land_normal, lift_offset, land_offset, | ||
| 248 | lift_offset_duration, land_offset_duration)), | ||
| 249 | 1 | to_quat_((quatWayPointsBegin->second).data()), | |
| 250 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | land_quat_(((quatWayPointsEnd - 1)->second).data()), |
| 251 | 1 | time_lift_offset_(spline_->min() + lift_offset_duration), | |
| 252 | 1 | time_land_offset_(spline_->max() - land_offset_duration), | |
| 253 | 2 | quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) { | |
| 254 | // NOTHING | ||
| 255 | 1 | } | |
| 256 | |||
| 257 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
6 | 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*/ | ||
| 266 | /// \brief Evaluation of the effector position and rotation at time t. | ||
| 267 | /// \param t : the time when to evaluate the spline. | ||
| 268 | /// \return A 7D vector where the 3 first values are the 3D position and the | ||
| 269 | /// 4 last are the quaternion describing the rotation. | ||
| 270 | /// | ||
| 271 | 15 | config_t operator()(const Numeric t) const { | |
| 272 | 15 | config_t res; | |
| 273 |
3/6✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
|
15 | res.head<3>() = (*spline_)(t); |
| 274 |
3/6✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
|
15 | res.tail<4>() = interpolate_quat(t); |
| 275 | 15 | return res; | |
| 276 | } | ||
| 277 | |||
| 278 | 15 | quat_t interpolate_quat(const Numeric t) const { | |
| 279 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 9 times.
|
15 | if (t <= time_lift_offset_) return to_quat_.coeffs(); |
| 280 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 3 times.
|
9 | if (t >= time_land_offset_) return land_quat_.coeffs(); |
| 281 | 3 | return quat_spline_(t); | |
| 282 | } | ||
| 283 | |||
| 284 | private: | ||
| 285 | 2 | exact_cubic_quat_t simple_quat_spline() const { | |
| 286 | 2 | std::vector<rotation_spline> splines; | |
| 287 |
4/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
|
2 | splines.push_back(rotation_spline(to_quat_.coeffs(), land_quat_.coeffs(), |
| 288 | 2 | time_lift_offset_, time_land_offset_)); | |
| 289 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return exact_cubic_quat_t(splines); |
| 290 | 2 | } | |
| 291 | |||
| 292 | template <typename InQuat> | ||
| 293 | 1 | exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, | |
| 294 | InQuat quatWayPointsEnd) const { | ||
| 295 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) { |
| 296 | ✗ | return simple_quat_spline(); | |
| 297 | } | ||
| 298 | 1 | exact_cubic_quat_t::t_spline_t splines; | |
| 299 | 1 | InQuat it(quatWayPointsBegin); | |
| 300 | 1 | Time init = time_lift_offset_; | |
| 301 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond current_quat = to_quat_; |
| 302 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
4 | for (; it != quatWayPointsEnd; ++it) { |
| 303 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | splines.push_back( |
| 304 |
3/6✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
|
6 | rotation_spline(current_quat.coeffs(), it->second, init, it->first)); |
| 305 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | current_quat = it->second; |
| 306 | 3 | init = it->first; | |
| 307 | } | ||
| 308 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | splines.push_back(rotation_spline( |
| 309 | 2 | current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_)); | |
| 310 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | return exact_cubic_quat_t(splines); |
| 311 | 1 | } | |
| 312 | /*Operations*/ | ||
| 313 | |||
| 314 | public: | ||
| 315 | /*Attributes*/ | ||
| 316 | const exact_cubic_t* spline_; | ||
| 317 | const Eigen::Quaterniond to_quat_; | ||
| 318 | const Eigen::Quaterniond land_quat_; | ||
| 319 | const double time_lift_offset_; | ||
| 320 | const double time_land_offset_; | ||
| 321 | const exact_cubic_quat_t quat_spline_; | ||
| 322 | /*Attributes*/ | ||
| 323 | }; // End class effector_spline_rotation | ||
| 324 | |||
| 325 | } // namespace helpers | ||
| 326 | } // namespace ndcurves | ||
| 327 | #endif //_CLASS_EFFECTOR_SPLINE_ROTATION | ||
| 328 |