se3_curve.h 11.3 KB
 Pierre Fernbach committed Oct 09, 2019 1 2 3 4 5 6 7 8 9 10 #ifndef _STRUCT_SE3_CURVE_H #define _STRUCT_SE3_CURVE_H #include "MathDefs.h" #include "curve_abc.h" #include "so3_linear.h" #include "polynomial.h" #include #include  Guilhem Saurel committed Nov 01, 2019 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 namespace curves { /// \class SE3Curve. /// \brief Composition of a curve of any type of dimension 3 and a curve representing an rotation /// (in current implementation, only SO3Linear can be used for the rotation part) /// The output is a vector of size 7 (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w) /// The output of the derivative of any order is a vector of size 6 /// (linear_x,linear_y,linear_z,angular_x,angular_y,angular_z) /// /// template struct SE3Curve : public curve_abc, Eigen::Matrix > { typedef Numeric Scalar; typedef Eigen::Transform transform_t; typedef transform_t point_t; typedef Eigen::Matrix point_derivate_t; typedef Eigen::Matrix point3_t; typedef Eigen::Matrix pointX_t; typedef Eigen::Matrix matrix3_t; typedef Eigen::Quaternion Quaternion; typedef Time time_t; typedef curve_abc curve_abc_t; // parent class typedef curve_abc curve_X_t; // generic class of curve  Guilhem Saurel committed Jan 10, 2020 35 36  typedef curve_abc curve_rotation_t; // templated class used for the rotation (return dimension are fixed)  37 38 39  typedef boost::shared_ptr curve_ptr_t; typedef boost::shared_ptr curve_rotation_ptr_t;  Guilhem Saurel committed Nov 01, 2019 40 41 42 43 44 45 46  typedef SO3Linear SO3Linear_t; typedef polynomial polynomial_t; typedef SE3Curve SE3Curve_t; public: /* Constructors - destructors */ /// \brief Empty constructor. Curve obtained this way can not perform other class functions.  Pierre Fernbach committed Oct 09, 2019 47  ///  48  SE3Curve() : curve_abc_t(), dim_(3), translation_curve_(), rotation_curve_(), T_min_(0), T_max_(0) {}  Guilhem Saurel committed Nov 01, 2019 49 50 51 52 53 54 55 56 57 58 59  /// \brief Destructor ~SE3Curve() { // should we delete translation_curve and rotation_curve here ? // better switch to shared ptr } /* Constructor without curve object for the translation : */ /// \brief Constructor from init/end transform use polynomial of degree 1 for position and SO3Linear for rotation SE3Curve(const transform_t& init_transform, const transform_t& end_transform, const time_t& t_min, const time_t& t_max)  Pierre Fernbach committed Oct 09, 2019 60 61  : curve_abc_t(), dim_(6),  Guilhem Saurel committed Nov 01, 2019 62 63 64 65 66 67 68 69 70 71 72 73  translation_curve_(new polynomial_t(pointX_t(init_transform.translation()), pointX_t(end_transform.translation()), t_min, t_max)), rotation_curve_(new SO3Linear_t(init_transform.rotation(), end_transform.rotation(), t_min, t_max)), T_min_(t_min), T_max_(t_max) { safe_check(); } /// \brief Constructor from init/end pose, with quaternion. use polynomial of degree 1 for position and SO3Linear for /// rotation SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const Quaternion& init_rot, const Quaternion& end_rot, const time_t& t_min, const time_t& t_max)  Pierre Fernbach committed Oct 09, 2019 74  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 75 76 77 78 79 80 81 82 83 84 85 86  dim_(6), translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)), rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)), T_min_(t_min), T_max_(t_max) { safe_check(); } /// \brief Constructor from init/end pose, with rotation matrix. use polynomial of degree 1 for position and /// SO3Linear for rotation SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const matrix3_t& init_rot, const matrix3_t& end_rot, const time_t& t_min, const time_t& t_max)  Pierre Fernbach committed Oct 09, 2019 87  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 88 89 90 91 92 93 94 95 96 97 98  dim_(6), translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)), rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)), T_min_(t_min), T_max_(t_max) { safe_check(); } /* Constructor with curve object for the translation : */ /// \brief Constructor from curve for the translation and init/end rotation, with quaternion. /// Use SO3Linear for rotation with the same time bounds as the  99  SE3Curve(curve_ptr_t translation_curve, const Quaternion& init_rot, const Quaternion& end_rot)  Pierre Fernbach committed Oct 09, 2019 100  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 101 102 103 104 105 106 107 108 109  dim_(6), translation_curve_(translation_curve), rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())), T_min_(translation_curve->min()), T_max_(translation_curve->max()) { safe_check(); } /// \brief Constructor from curve for the translation and init/end rotation, with rotation matrix. /// Use SO3Linear for rotation with the same time bounds as the  110  SE3Curve(curve_ptr_t translation_curve, const matrix3_t& init_rot, const matrix3_t& end_rot)  Pierre Fernbach committed Oct 09, 2019 111  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 112 113 114 115 116 117 118 119 120 121  dim_(6), translation_curve_(translation_curve), rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())), T_min_(translation_curve->min()), T_max_(translation_curve->max()) { safe_check(); } /* Constructor from translation and rotation curves object : */ /// \brief Constructor from from translation and rotation curves object  122  SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)  Pierre Fernbach committed Oct 09, 2019 123  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 124 125  dim_(6), translation_curve_(translation_curve),  Pierre Fernbach committed Oct 09, 2019 126  rotation_curve_(rotation_curve),  Guilhem Saurel committed Nov 01, 2019 127 128 129 130  T_min_(translation_curve->min()), T_max_(translation_curve->max()) { if (translation_curve->dim() != 3) { throw std::invalid_argument("The translation curve should be of dimension 3.");  Pierre Fernbach committed Oct 09, 2019 131  }  Guilhem Saurel committed Nov 01, 2019 132 133  if (rotation_curve->min() != T_min_) { throw std::invalid_argument("Min bounds of translation and rotation curve are not the same.");  Pierre Fernbach committed Oct 09, 2019 134  }  Guilhem Saurel committed Nov 01, 2019 135 136  if (rotation_curve->max() != T_max_) { throw std::invalid_argument("Max bounds of translation and rotation curve are not the same.");  Pierre Fernbach committed Oct 09, 2019 137  }  Guilhem Saurel committed Nov 01, 2019 138 139 140 141 142 143 144 145 146  safe_check(); } /// \brief Evaluation of the SE3Curve at time t /// \param t : time when to evaluate the spline. /// \return \f$x(t)\f$ point corresponding on spline at time t. (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w) virtual point_t operator()(const time_t t) const { if (translation_curve_->dim() != 3) { throw std::invalid_argument("Translation curve should always be of dimension 3");  Pierre Fernbach committed Oct 09, 2019 147  }  Guilhem Saurel committed Nov 01, 2019 148 149 150 151 152 153  point_t res = point_t::Identity(); res.translate(point3_t((*translation_curve_)(t))); res.rotate((*rotation_curve_)(t)); return res; }  Pierre Fernbach committed Dec 17, 2019 154  /**  Pierre Fernbach committed Dec 20, 2019 155  * @brief isApprox check if other and *this are approximately equals.  Guilhem Saurel committed Jan 10, 2020 156 157  * Only two curves of the same class can be approximately equals, for comparison between different type of curves see * isEquivalent  Pierre Fernbach committed Dec 17, 2019 158 159 160 161  * @param other the other curve to check * @param prec the precision treshold, default Eigen::NumTraits::dummy_precision() * @return true is the two curves are approximately equals */  Guilhem Saurel committed Jan 10, 2020 162 163 164 165 166  bool isApprox(const SE3Curve_t& other, const Numeric prec = Eigen::NumTraits::dummy_precision()) const { return curves::isApprox(T_min_, other.min()) && curves::isApprox(T_max_, other.max()) && (translation_curve_ == other.translation_curve_ || translation_curve_->isApprox(other.translation_curve_.get(), prec)) && (rotation_curve_ == other.rotation_curve_ || rotation_curve_->isApprox(other.rotation_curve_.get(), prec));  Pierre Fernbach committed Dec 17, 2019 167 168  }  Guilhem Saurel committed Jan 10, 2020 169 170  virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits::dummy_precision()) const {  Pierre Fernbach committed Dec 20, 2019 171  const SE3Curve_t* other_cast = dynamic_cast(other);  Guilhem Saurel committed Jan 10, 2020 172 173  if (other_cast) return isApprox(*other_cast, prec);  Pierre Fernbach committed Dec 20, 2019 174 175 176 177  else return false; }  Guilhem Saurel committed Jan 10, 2020 178  virtual bool operator==(const SE3Curve_t& other) const { return isApprox(other); }  Pierre Fernbach committed Dec 20, 2019 179   Guilhem Saurel committed Jan 10, 2020 180  virtual bool operator!=(const SE3Curve_t& other) const { return !(*this == other); }  Pierre Fernbach committed Dec 17, 2019 181   Guilhem Saurel committed Nov 01, 2019 182 183 184 185 186 187 188 189 190 191 192 193 194 195  /// \brief Evaluation of the derivative of order N of spline at time t. /// \param t : the time when to evaluate the spline. /// \param order : order of derivative. /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t. virtual point_derivate_t derivate(const time_t t, const std::size_t order) const { if (translation_curve_->dim() != 3) { throw std::invalid_argument("Translation curve should always be of dimension 3"); } point_derivate_t res = point_derivate_t::Zero(); res.segment(0, 3) = point3_t(translation_curve_->derivate(t, order)); res.segment(3, 3) = rotation_curve_->derivate(t, order); return res; }  196  SE3Curve_t compute_derivate(const std::size_t /*order*/) const {  197 198 199  throw std::logic_error("Compute derivate for SE3 is not implemented yet."); }  200 201 202  /// \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.  Guilhem Saurel committed Jan 10, 2020 203  SE3Curve_t* compute_derivate_ptr(const std::size_t order) const { return new SE3Curve_t(compute_derivate(order)); }  204   Guilhem Saurel committed Nov 01, 2019 205 206 207 208 209 210 211 212 213 214  /*Helpers*/ /// \brief Get dimension of curve. /// \return dimension of curve. std::size_t virtual dim() const { return dim_; }; /// \brief Get the minimum time for which the curve is defined /// \return \f$t_{min}\f$ lower bound of time range. time_t min() const { return T_min_; } /// \brief Get the maximum time for which the curve is defined. /// \return \f$t_{max}\f$ upper bound of time range. time_t max() const { return T_max_; }  Pierre Fernbach committed Nov 30, 2019 215 216  /// \brief Get the degree of the curve. /// \return \f$degree\f$, the degree of the curve.  Guilhem Saurel committed Jan 10, 2020 217  virtual std::size_t degree() const { return translation_curve_->degree(); }  Pierre Fernbach committed Jan 23, 2020 218  /// \brief const accessor to the translation curve  Pierre Fernbach committed Jan 23, 2020 219  const curve_ptr_t translation_curve() const {return translation_curve_;}  Pierre Fernbach committed Jan 23, 2020 220  /// \brief const accessor to the rotation curve  Pierre Fernbach committed Jan 23, 2020 221  const curve_rotation_ptr_t rotation_curve() const {return rotation_curve_;}  Guilhem Saurel committed Nov 01, 2019 222 223 224 225  /*Helpers*/ /*Attributes*/ std::size_t dim_; // dim doesn't mean anything in this class ...  226 227  curve_ptr_t translation_curve_; curve_rotation_ptr_t rotation_curve_;  Guilhem Saurel committed Nov 01, 2019 228 229 230 231 232 233 234 235 236 237 238  time_t T_min_, T_max_; /*Attributes*/ // Serialization of the class friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version) { if (version) { // Do something depending on version ? }  Pierre Fernbach committed Nov 29, 2019 239  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);  Guilhem Saurel committed Nov 01, 2019 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254  ar& boost::serialization::make_nvp("dim", dim_); ar& boost::serialization::make_nvp("translation_curve", translation_curve_); ar& boost::serialization::make_nvp("rotation_curve", rotation_curve_); ar& boost::serialization::make_nvp("T_min", T_min_); ar& boost::serialization::make_nvp("T_max", T_max_); } private: void safe_check() { if (Safe) { if (T_min_ > T_max_) { throw std::invalid_argument("Tmin should be inferior to Tmax"); } if (translation_curve_->dim() != 3) { throw std::invalid_argument("Translation curve should always be of dimension 3");  Pierre Fernbach committed Oct 09, 2019 255 256  } }  Guilhem Saurel committed Nov 01, 2019 257  }  Pierre Fernbach committed Oct 09, 2019 258   Guilhem Saurel committed Nov 01, 2019 259 }; // SE3Curve  Pierre Fernbach committed Oct 09, 2019 260   Guilhem Saurel committed Nov 01, 2019 261 } // namespace curves  Pierre Fernbach committed Oct 09, 2019 262   Guilhem Saurel committed Nov 01, 2019 263 #endif // SE3_CURVE_H