se3_curve.h 11.4 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  35 36 37 38  typedef curve_abc curve_rotation_t; // templated class used for the rotation (return dimension are fixed) typedef boost::shared_ptr curve_ptr_t; typedef boost::shared_ptr curve_rotation_ptr_t;  Guilhem Saurel committed Nov 01, 2019 39 40 41 42 43 44 45  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 46  ///  47  SE3Curve() : curve_abc_t(), dim_(3), translation_curve_(), rotation_curve_(), T_min_(0), T_max_(0) {}  Guilhem Saurel committed Nov 01, 2019 48 49 50 51 52 53 54 55 56 57 58  /// \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 59 60  : curve_abc_t(), dim_(6),  Guilhem Saurel committed Nov 01, 2019 61 62 63 64 65 66 67 68 69 70 71 72  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 73  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 74 75 76 77 78 79 80 81 82 83 84 85  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 86  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 87 88 89 90 91 92 93 94 95 96 97  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  98  SE3Curve(curve_ptr_t translation_curve, const Quaternion& init_rot, const Quaternion& end_rot)  Pierre Fernbach committed Oct 09, 2019 99  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 100 101 102 103 104 105 106 107 108  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  109  SE3Curve(curve_ptr_t translation_curve, const matrix3_t& init_rot, const matrix3_t& end_rot)  Pierre Fernbach committed Oct 09, 2019 110  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 111 112 113 114 115 116 117 118 119 120  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  121  SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)  Pierre Fernbach committed Oct 09, 2019 122  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 123 124  dim_(6), translation_curve_(translation_curve),  Pierre Fernbach committed Oct 09, 2019 125  rotation_curve_(rotation_curve),  Guilhem Saurel committed Nov 01, 2019 126 127 128 129  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 130  }  Guilhem Saurel committed Nov 01, 2019 131 132  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 133  }  Guilhem Saurel committed Nov 01, 2019 134 135  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 136  }  Guilhem Saurel committed Nov 01, 2019 137 138 139 140 141 142 143 144 145  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 146  }  Guilhem Saurel committed Nov 01, 2019 147 148 149 150 151 152  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 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  /** * @brief isApprox check if other and *this are equals, given a precision treshold. * This test is done by discretizing, it should be re-implemented in the child class to check exactly * all the members. * @param other the other curve to check * @param order the order up to which the derivatives of the curves are checked for equality * @param prec the precision treshold, default Eigen::NumTraits::dummy_precision() * @return true is the two curves are approximately equals */ virtual bool isApprox(const SE3Curve_t& other, const Numeric prec = Eigen::NumTraits::dummy_precision(),const size_t order = 5) const{ //std::cout<<"is approx in SE3 called."<isApprox(*other.translation_curve_,prec)) && (rotation_curve_ == other.rotation_curve_ || rotation_curve_->isApprox(*other.rotation_curve_,prec)); } virtual bool operator==(const SE3Curve_t& other) const { return isApprox(other); } virtual bool operator!=(const SE3Curve_t& other) const { return !(*this == other); }  179 180 181 182 183 184 185  virtual bool isApprox(const curve_abc_t& other, const Numeric prec = Eigen::NumTraits::dummy_precision(),const size_t order = 5) const{ const SE3Curve_t* other_cast = dynamic_cast(&other); if(other_cast) return isApprox(*other_cast); else return curve_abc_t::isApprox(other,prec,order); }  Pierre Fernbach committed Dec 17, 2019 186  virtual bool operator==(const curve_abc_t& other) const {  187  return isApprox(other);  Pierre Fernbach committed Dec 17, 2019 188 189 190  } virtual bool operator!=(const curve_abc_t& other) const {  191  return !(*this == other);  Pierre Fernbach committed Dec 17, 2019 192 193 194  }  Guilhem Saurel committed Nov 01, 2019 195 196 197 198 199 200 201 202 203 204 205 206 207 208  /// \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; }  209  SE3Curve_t compute_derivate(const std::size_t /*order*/) const {  210 211 212  throw std::logic_error("Compute derivate for SE3 is not implemented yet."); }  213 214 215 216 217 218 219  /// \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. SE3Curve_t* compute_derivate_ptr(const std::size_t order) const { return new SE3Curve_t(compute_derivate(order)); }  Guilhem Saurel committed Nov 01, 2019 220 221 222 223 224 225 226 227 228 229  /*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 230 231 232  /// \brief Get the degree of the curve. /// \return \f$degree\f$, the degree of the curve. virtual std::size_t degree() const {return translation_curve_->degree();}  Guilhem Saurel committed Nov 01, 2019 233 234 235 236  /*Helpers*/ /*Attributes*/ std::size_t dim_; // dim doesn't mean anything in this class ...  237 238  curve_ptr_t translation_curve_; curve_rotation_ptr_t rotation_curve_;  Guilhem Saurel committed Nov 01, 2019 239 240 241 242 243 244 245 246 247 248 249  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 250  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);  Guilhem Saurel committed Nov 01, 2019 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265  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 266 267  } }  Guilhem Saurel committed Nov 01, 2019 268  }  Pierre Fernbach committed Oct 09, 2019 269   Guilhem Saurel committed Nov 01, 2019 270 }; // SE3Curve  Pierre Fernbach committed Oct 09, 2019 271   Guilhem Saurel committed Nov 01, 2019 272 } // namespace curves  Pierre Fernbach committed Oct 09, 2019 273   Guilhem Saurel committed Nov 01, 2019 274 #endif // SE3_CURVE_H